diff --git a/flight/libraries/auxmagsupport.c b/flight/libraries/auxmagsupport.c new file mode 100644 index 000000000..3d27e336a --- /dev/null +++ b/flight/libraries/auxmagsupport.c @@ -0,0 +1,72 @@ +/** + ****************************************************************************** + * + * @file auxmagsupport.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Functions to handle aux mag data and calibration. + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include +#include "inc/auxmagsupport.h" +#include "CoordinateConversions.h" + +static float mag_bias[3] = { 0, 0, 0 }; +static float mag_transform[3][3] = { + { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } +}; + +AuxMagSettingsTypeOptions option; + +void auxmagsupport_reload_settings() +{ + AuxMagSettingsTypeGet(&option); + float a[3][3]; + float b[3][3]; + float rotz; + AuxMagSettingsmag_transformArrayGet((float *)a); + AuxMagSettingsOrientationGet(&rotz); + rotz = DEG2RAD(rotz); + rot_about_axis_z(rotz, b); + matrix_mult_3x3f(a, b, mag_transform); + AuxMagSettingsmag_biasArrayGet(mag_bias); +} + +void auxmagsupport_publish_samples(float mags[3], uint8_t status) +{ + float mag_out[3]; + + mags[0] -= mag_bias[0]; + mags[1] -= mag_bias[1]; + mags[2] -= mag_bias[2]; + rot_mult(mag_transform, mags, mag_out); + + AuxMagSensorData data; + data.x = mag_out[0]; + data.y = mag_out[1]; + data.z = mag_out[2]; + data.Status = status; + AuxMagSensorSet(&data); +} + +AuxMagSettingsTypeOptions auxmagsupport_get_type() +{ + return option; +} diff --git a/flight/libraries/inc/auxmagsupport.h b/flight/libraries/inc/auxmagsupport.h new file mode 100644 index 000000000..90b063e9e --- /dev/null +++ b/flight/libraries/inc/auxmagsupport.h @@ -0,0 +1,51 @@ +/** + ****************************************************************************** + * + * @file auxmagsupport.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Functions to handle aux mag data and calibration. + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef AUXMAGSUPPORT_H_ +#define AUXMAGSUPPORT_H_ +#include +#include +#include +/** + * @brief reload Aux Mag settings + */ +void auxmagsupport_reload_settings(); + +/** + * @brief Publish a new Aux Magnetometer sample + * @param[in] mags Mag sample in milliGauss + * @param[in] status one of AuxMagSensorStatusOptions option + */ +void auxmagsupport_publish_samples(float mags[3], uint8_t status); + +/** + * @brief Get the Aux Mag settings Type option + * @param[in] mags Mag sample in milliGauss + * @return one of AuxMagSettingsTypeOptions option + */ +AuxMagSettingsTypeOptions auxmagsupport_get_type(); + + +#endif /* AUXMAGSUPPORT_H_ */ diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index bdcbfaed6..e476dbb85 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -76,6 +76,7 @@ ifndef TESTAPP SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(FLIGHTLIB)/auxmagsupport.c ## UAVObjects SRC += $(OPUAVSYNTHDIR)/accessorydesired.c diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index bc8e72959..bf7d21d28 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -86,6 +86,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/auxmagsupport.c ## UAVObjects include ./UAVObjects.inc diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile index 6ff4d37cb..6bc37c1d8 100644 --- a/flight/targets/boards/osd/firmware/Makefile +++ b/flight/targets/boards/osd/firmware/Makefile @@ -70,6 +70,7 @@ ifndef TESTAPP ## Misc library functions SRC += $(FLIGHTLIB)/WorldMagModel.c + SRC += $(FLIGHTLIB)/auxmagsupport.c ## UAVObjects SRC += $(OPUAVSYNTHDIR)/objectpersistence.c diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 52b5f8a64..21b23ed68 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -87,6 +87,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/auxmagsupport.c ## UAVObjects include ./UAVObjects.inc diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 9933124a3..f8f0f5336 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -85,6 +85,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/auxmagsupport.c ## UAVObjects include ./UAVObjects.inc