flight/libraries/auxmagsupport.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       auxmagsupport.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
   6  * @brief      Functions to handle aux mag data and calibration.
   7  *             --
   8  * @see        The GNU Public License (GPL) Version 3
   9  *
  10  *****************************************************************************/
  11 /*
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20  * for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License along
  23  * with this program; if not, write to the Free Software Foundation, Inc.,
  24  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25  */
  26
  27 #include <stdint.h>
  28 #include "inc/auxmagsupport.h"
  29 #include "CoordinateConversions.h"
  30
  31 static float mag_bias[3] = { 0, 0, 0 };
  32 static float mag_transform[3][3] = {
  33     { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
  34 };
  35
  36 AuxMagSettingsTypeOptions option;
  37
  38 void auxmagsupport_reload_settings()
  39 {
  40     AuxMagSettingsTypeGet(&option);
  41     float a[3][3];
  42     float b[3][3];
  43     float rotz;
  44     AuxMagSettingsmag_transformArrayGet((float *)a);
  45     AuxMagSettingsOrientationGet(&rotz);
  46     rotz = DEG2RAD(rotz);
  47     rot_about_axis_z(rotz, b);
  48     matrix_mult_3x3f(a, b, mag_transform);
  49     AuxMagSettingsmag_biasArrayGet(mag_bias);
  50 }
  51
  52 void auxmagsupport_publish_samples(float mags[3], uint8_t status)
  53 {
  54     float mag_out[3];
  55
  56     mags[0] -= mag_bias[0];
  57     mags[1] -= mag_bias[1];
  58     mags[2] -= mag_bias[2];
  59     rot_mult(mag_transform, mags, mag_out);
  60
  61     AuxMagSensorData data;
  62     data.x = mag_out[0];
  63     data.y = mag_out[1];
  64     data.z = mag_out[2];
  65     data.Status = status;
  66     AuxMagSensorSet(&data);
  67 }
  68
  69 AuxMagSettingsTypeOptions auxmagsupport_get_type()
  70 {
  71     return option;
  72 }