flight/libraries/auxmagsupport.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       auxmagsupport.c
   5  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
   6  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
   7  * @brief      Functions to handle aux mag data and calibration.
   8  *             --
   9  * @see        The GNU Public License (GPL) Version 3
  10  *
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27
  28 #include <stdint.h>
  29 #include "inc/auxmagsupport.h"
  30 #include "CoordinateConversions.h"
  31
  32 static float mag_bias[3] = { 0, 0, 0 };
  33 static float mag_transform[3][3] = {
  34     { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
  35 };
  36
  37 AuxMagSettingsTypeOptions option;
  38
  39 void auxmagsupport_reload_settings()
  40 {
  41     AuxMagSettingsData cal;
  42     float magQuat[4];
  43     float R[3][3];
  44
  45     AuxMagSettingsGet(&cal);
  46     mag_bias[0] = cal.mag_bias.X;
  47     mag_bias[1] = cal.mag_bias.Y;
  48     mag_bias[2] = cal.mag_bias.Z;
  49
  50     // convert the RPY mag board rotation to into a rotation matrix
  51     // rotate the vector into the level hover frame (the attitude frame)
  52     const float magRpy[3] = { cal.BoardRotation.Roll, cal.BoardRotation.Pitch, cal.BoardRotation.Yaw };
  53     RPY2Quaternion(magRpy, magQuat);
  54     Quaternion2R(magQuat, R);
  55
  56     // the mag transform only scales the raw mag values
  57     matrix_mult_3x3f((float(*)[3])AuxMagSettingsmag_transformToArray(cal.mag_transform), R, mag_transform);
  58
  59     // GPSV9, Ext (unused), and Flexi
  60     AuxMagSettingsTypeGet(&option);
  61
  62     const uint8_t status = AUXMAGSENSOR_STATUS_NONE;
  63     // next sample from other external mags will provide the right status if present
  64     AuxMagSensorStatusSet((uint8_t *)&status);
  65 }
  66
  67 void auxmagsupport_publish_samples(float mags[3], uint8_t status)
  68 {
  69     float mag_out[3];
  70
  71     mags[0] -= mag_bias[0];
  72     mags[1] -= mag_bias[1];
  73     mags[2] -= mag_bias[2];
  74     rot_mult(mag_transform, mags, mag_out);
  75
  76     AuxMagSensorData data;
  77     data.x = mag_out[0];
  78     data.y = mag_out[1];
  79     data.z = mag_out[2];
  80     data.Status = status;
  81     AuxMagSensorSet(&data);
  82 }
  83
  84 AuxMagSettingsTypeOptions auxmagsupport_get_type()
  85 {
  86     return option;
  87 }