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 <stdint.h>
+#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/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h
index c8a391f43..e9369bd26 100644
--- a/flight/libraries/inc/CoordinateConversions.h
+++ b/flight/libraries/inc/CoordinateConversions.h
@@ -112,5 +112,73 @@ static inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3
result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2];
}
+inline void matrix_inline_scale_3f(float a[3][3], float scale)
+{
+ a[0][0] *= scale;
+ a[0][1] *= scale;
+ a[0][2] *= scale;
+
+ a[1][0] *= scale;
+ a[1][1] *= scale;
+ a[1][2] *= scale;
+
+ a[2][0] *= scale;
+ a[2][1] *= scale;
+ a[2][2] *= scale;
+}
+
+inline void rot_about_axis_x(const float rotation, float R[3][3])
+{
+ float s = sinf(rotation);
+ float c = cosf(rotation);
+
+ R[0][0] = 1;
+ R[0][1] = 0;
+ R[0][2] = 0;
+
+ R[1][0] = 0;
+ R[1][1] = c;
+ R[1][2] = -s;
+
+ R[2][0] = 0;
+ R[2][1] = s;
+ R[2][2] = c;
+}
+
+inline void rot_about_axis_y(const float rotation, float R[3][3])
+{
+ float s = sinf(rotation);
+ float c = cosf(rotation);
+
+ R[0][0] = c;
+ R[0][1] = 0;
+ R[0][2] = s;
+
+ R[1][0] = 0;
+ R[1][1] = 1;
+ R[1][2] = 0;
+
+ R[2][0] = -s;
+ R[2][1] = 0;
+ R[2][2] = c;
+}
+
+inline void rot_about_axis_z(const float rotation, float R[3][3])
+{
+ float s = sinf(rotation);
+ float c = cosf(rotation);
+
+ R[0][0] = c;
+ R[0][1] = -s;
+ R[0][2] = 0;
+
+ R[1][0] = s;
+ R[1][1] = c;
+ R[1][2] = 0;
+
+ R[2][0] = 0;
+ R[2][1] = 0;
+ R[2][2] = 1;
+}
#endif // COORDINATECONVERSIONS_H_
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 <openpilot.h>
+#include <auxmagsettings.h>
+#include <auxmagsensor.h>
+/**
+ * @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/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c
index 382f1ba00..9729c5189 100644
--- a/flight/modules/GPS/GPS.c
+++ b/flight/modules/GPS/GPS.c
@@ -40,26 +40,36 @@
#include "gpssettings.h"
#include "taskinfo.h"
#include "hwsettings.h"
-
+#include "auxmagsensor.h"
#include "WorldMagModel.h"
#include "CoordinateConversions.h"
+#include <pios_com.h>
#include "GPS.h"
#include "NMEA.h"
#include "UBX.h"
-
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+#include "inc/ubx_autoconfig.h"
+#endif
// ****************
// Private functions
static void gpsTask(void *parameters);
-static void updateSettings();
+static void updateHwSettings();
#ifdef PIOS_GPS_SETS_HOMELOCATION
static void setHomeLocation(GPSPositionSensorData *gpsData);
static float GravityAccel(float latitude, float longitude, float altitude);
#endif
+#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
+void AuxMagSettingsUpdatedCb(UAVObjEvent *ev);
+#ifndef PIOS_GPS_MINIMAL
+void updateGpsSettings(UAVObjEvent *ev);
+#endif
+#endif
+
// ****************
// Private constants
@@ -153,8 +163,16 @@ int32_t GPSInitialize(void)
GPSTimeInitialize();
GPSSatellitesInitialize();
HomeLocationInitialize();
- updateSettings();
+#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
+ AuxMagSensorInitialize();
+ AuxMagSettingsInitialize();
+ GPSExtendedStatusInitialize();
+ // Initialize mag parameters
+ AuxMagSettingsUpdatedCb(NULL);
+ AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb);
+#endif
+ updateHwSettings();
#else
if (gpsPort && gpsEnabled) {
GPSPositionSensorInitialize();
@@ -166,7 +184,7 @@ int32_t GPSInitialize(void)
#if defined(PIOS_GPS_SETS_HOMELOCATION)
HomeLocationInitialize();
#endif
- updateSettings();
+ updateHwSettings();
}
#endif /* if defined(REVOLUTION) */
@@ -184,7 +202,9 @@ int32_t GPSInitialize(void)
gps_rx_buffer = NULL;
}
PIOS_Assert(gps_rx_buffer);
-
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+ GPSSettingsConnectCallback(updateGpsSettings);
+#endif
return 0;
}
@@ -215,10 +235,23 @@ static void gpsTask(__attribute__((unused)) void *parameters)
timeOfLastCommandMs = timeNowMs;
GPSPositionSensorGet(&gpspositionsensor);
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+ updateGpsSettings(0);
+#endif
// Loop forever
while (1) {
uint8_t c;
-
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+ if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
+ char *buffer = 0;
+ uint16_t count = 0;
+ ubx_autoconfig_run(&buffer, &count);
+ // Something to send?
+ if (count) {
+ PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
+ }
+ }
+#endif
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) {
int res;
@@ -230,8 +263,18 @@ static void gpsTask(__attribute__((unused)) void *parameters)
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case GPSSETTINGS_DATAPROTOCOL_UBX:
+ {
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+ int32_t ac_status = ubx_autoconfig_get_status();
+ gpspositionsensor.AutoConfigStatus =
+ ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
+ ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
+ ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
+#endif
res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
- break;
+ }
+ break;
#endif
default:
res = NO_PARSER; // this should not happen
@@ -257,7 +300,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
// we appear to be receiving GPS sentences OK, we've had an update
// criteria for GPS-OK taken from this post...
// http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
- if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSattelites) &&
+ if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) &&
(gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
@@ -346,7 +389,7 @@ static void setHomeLocation(GPSPositionSensorData *gpsData)
* like protocol, etc. Thus the HwSettings object which contains the
* GPS port speed is used for now.
*/
-static void updateSettings()
+static void updateHwSettings()
{
if (gpsPort) {
// Retrieve settings
@@ -376,10 +419,92 @@ static void updateSettings()
case HWSETTINGS_GPSSPEED_115200:
PIOS_COM_ChangeBaud(gpsPort, 115200);
break;
+ case HWSETTINGS_GPSSPEED_230400:
+ PIOS_COM_ChangeBaud(gpsPort, 230400);
+ break;
}
}
}
+#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
+void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+ load_mag_settings();
+}
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
+{
+ uint8_t ubxAutoConfig;
+ uint8_t ubxDynamicModel;
+ uint8_t ubxSbasMode;
+ ubx_autoconfig_settings_t newconfig;
+ uint8_t ubxSbasSats;
+
+ GPSSettingsUbxRateGet(&newconfig.navRate);
+
+ GPSSettingsUbxAutoConfigGet(&ubxAutoConfig);
+ newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true;
+ newconfig.storeSettings = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE;
+
+ GPSSettingsUbxDynamicModelGet(&ubxDynamicModel);
+ newconfig.dynamicModel = ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
+ ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY :
+ ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :
+ ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE :
+ ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA :
+ ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G :
+ ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G :
+ ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G :
+ UBX_DYNMODEL_AIRBORNE1G;
+
+ GPSSettingsUbxSBASModeGet(&ubxSbasMode);
+ switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
+ case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
+ case GPSSETTINGS_UBXSBASMODE_CORRECTION:
+ case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
+ case GPSSETTINGS_UBXSBASMODE_CORRECTIONINTEGRITY:
+ newconfig.SBASCorrection = true;
+ break;
+ default:
+ newconfig.SBASCorrection = false;
+ }
+
+ switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
+ case GPSSETTINGS_UBXSBASMODE_RANGING:
+ case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
+ case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
+ case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
+ newconfig.SBASRanging = true;
+ break;
+ default:
+ newconfig.SBASRanging = false;
+ }
+
+ switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
+ case GPSSETTINGS_UBXSBASMODE_INTEGRITY:
+ case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
+ case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
+ case GPSSETTINGS_UBXSBASMODE_CORRECTIONINTEGRITY:
+ newconfig.SBASIntegrity = true;
+ break;
+ default:
+ newconfig.SBASIntegrity = false;
+ }
+
+ GPSSettingsUbxSBASChannelsUsedGet(&newconfig.SBASChannelsUsed);
+
+ GPSSettingsUbxSBASSatsGet(&ubxSbasSats);
+
+ newconfig.SBASSats = ubxSbasSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS :
+ ubxSbasSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS :
+ ubxSbasSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS :
+ ubxSbasSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN :
+ ubxSbasSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN;
+
+ ubx_autoconfig_set(newconfig);
+}
+#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
+#endif /* ifdef PIOS_INCLUDE_GPS_UBX_PARSER */
/**
* @}
* @}
diff --git a/flight/modules/GPS/NMEA.c b/flight/modules/GPS/NMEA.c
index 7aac3b5c2..e45702990 100644
--- a/flight/modules/GPS/NMEA.c
+++ b/flight/modules/GPS/NMEA.c
@@ -489,7 +489,7 @@ static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdate
// geoid separation
GpsData->GeoidSeparation = NMEA_real_to_float(param[11]);
-
+ GpsData->SensorType = GPSPOSITIONSENSOR_SENSORTYPE_NMEA;
return true;
}
diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c
index 86910ef0d..ba417b5ac 100644
--- a/flight/modules/GPS/UBX.c
+++ b/flight/modules/GPS/UBX.c
@@ -30,10 +30,76 @@
#include "openpilot.h"
#include "pios.h"
-
+#include "pios_math.h"
+#include <pios_helpers.h>
+#include <pios_delay.h>
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
#include "inc/UBX.h"
#include "inc/GPS.h"
+#include <string.h>
+#include <auxmagsupport.h>
+
+static bool useMag = false;
+static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
+
+static bool usePvt = false;
+static uint32_t lastPvtTime = 0;
+
+
+// parse table item
+typedef struct {
+ uint8_t msgClass;
+ uint8_t msgID;
+ void (*handler)(struct UBXPacket *, GPSPositionSensorData *GpsPosition);
+} ubx_message_handler;
+
+// parsing functions, roughly ordered by reception rate (higher rate messages on top)
+static void parse_ubx_op_mag(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+
+static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+static void parse_ubx_nav_posllh(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+static void parse_ubx_nav_velned(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+static void parse_ubx_nav_sol(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+static void parse_ubx_nav_dop(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+#ifndef PIOS_GPS_MINIMAL
+static void parse_ubx_nav_timeutc(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+
+static void parse_ubx_op_sys(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+#endif
+static void parse_ubx_ack_ack(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+static void parse_ubx_ack_nak(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+
+static void parse_ubx_mon_ver(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+
+
+const ubx_message_handler ubx_handler_table[] = {
+ { .msgClass = UBX_CLASS_OP_CUST, .msgID = UBX_ID_OP_MAG, .handler = &parse_ubx_op_mag },
+
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_PVT, .handler = &parse_ubx_nav_pvt },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .handler = &parse_ubx_nav_posllh },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED, .handler = &parse_ubx_nav_velned },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL, .handler = &parse_ubx_nav_sol },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .handler = &parse_ubx_nav_dop },
+#ifndef PIOS_GPS_MINIMAL
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .handler = &parse_ubx_nav_svinfo },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEUTC, .handler = &parse_ubx_nav_timeutc },
+
+ { .msgClass = UBX_CLASS_OP_CUST, .msgID = UBX_ID_OP_SYS, .handler = &parse_ubx_op_sys },
+#endif
+ { .msgClass = UBX_CLASS_ACK, .msgID = UBX_ID_ACK_ACK, .handler = &parse_ubx_ack_ack },
+ { .msgClass = UBX_CLASS_ACK, .msgID = UBX_ID_ACK_NAK, .handler = &parse_ubx_ack_nak },
+
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_VER, .handler = &parse_ubx_mon_ver },
+};
+#define UBX_HANDLER_TABLE_SIZE NELEMENTS(ubx_handler_table)
+
+// detected hw version
+int32_t ubxHwVersion = -1;
+
+// Last received Ack/Nak
+struct UBX_ACK_ACK ubxLastAck;
+struct UBX_ACK_NAK ubxLastNak;
// If a PVT sentence is received in the last UBX_PVT_TIMEOUT (ms) timeframe it disables VELNED/POSLLH/SOL/TIMEUTC
#define UBX_PVT_TIMEOUT (1000)
@@ -194,8 +260,13 @@ bool checksum_ubx_message(struct UBXPacket *ubx)
}
}
-void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionSensorData *GpsPosition)
+static void parse_ubx_nav_posllh(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
+ if (usePvt) {
+ return;
+ }
+ struct UBX_NAV_POSLLH *posllh = &ubx->payload.nav_posllh;
+
if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) {
if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) {
GpsPosition->Altitude = (float)posllh->hMSL * 0.001f;
@@ -206,8 +277,12 @@ void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionSensorData *
}
}
-void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionSensorData *GpsPosition)
+static void parse_ubx_nav_sol(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
+ if (usePvt) {
+ return;
+ }
+ struct UBX_NAV_SOL *sol = &ubx->payload.nav_sol;
if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) {
GpsPosition->Satellites = sol->numSV;
@@ -227,8 +302,10 @@ void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionSensorData *GpsPositi
}
}
-void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionSensorData *GpsPosition)
+static void parse_ubx_nav_dop(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
+ struct UBX_NAV_DOP *dop = &ubx->payload.nav_dop;
+
if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) {
GpsPosition->HDOP = (float)dop->hDOP * 0.01f;
GpsPosition->VDOP = (float)dop->vDOP * 0.01f;
@@ -236,10 +313,13 @@ void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionSensorData *GpsPositi
}
}
-void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *GpsPosition)
+static void parse_ubx_nav_velned(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
+ if (usePvt) {
+ return;
+ }
GPSVelocitySensorData GpsVelocity;
-
+ struct UBX_NAV_VELNED *velned = &ubx->payload.nav_velned;
if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) {
if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) {
GpsVelocity.North = (float)velned->velN / 100.0f;
@@ -252,10 +332,12 @@ void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *
}
}
-void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPosition)
+static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
- GPSVelocitySensorData GpsVelocity;
+ lastPvtTime = PIOS_DELAY_GetuS();
+ GPSVelocitySensorData GpsVelocity;
+ struct UBX_NAV_PVT *pvt = &ubx->payload.nav_pvt;
check_msgtracker(pvt->iTOW, (ALL_RECEIVED));
GpsVelocity.North = (float)pvt->velN * 0.001f;
@@ -293,9 +375,15 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi
}
#endif
}
+
#if !defined(PIOS_GPS_MINIMAL)
-void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc)
+static void parse_ubx_nav_timeutc(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
{
+ if (usePvt) {
+ return;
+ }
+
+ struct UBX_NAV_TIMEUTC *timeutc = &ubx->payload.nav_timeutc;
// Test if time is valid
if ((timeutc->valid & TIMEUTC_VALIDTOW) && (timeutc->valid & TIMEUTC_VALIDWKN)) {
// Time is valid, set GpsTime
@@ -317,10 +405,11 @@ void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc)
#endif /* if !defined(PIOS_GPS_MINIMAL) */
#if !defined(PIOS_GPS_MINIMAL)
-void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
+static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
{
uint8_t chan;
GPSSatellitesData svdata;
+ struct UBX_NAV_SVINFO *svinfo = &ubx->payload.nav_svinfo;
svdata.SatsInView = 0;
for (chan = 0; chan < svinfo->numCh; chan++) {
@@ -344,14 +433,64 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
}
#endif /* if !defined(PIOS_GPS_MINIMAL) */
+static void parse_ubx_ack_ack(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
+{
+ struct UBX_ACK_ACK *ack_ack = &ubx->payload.ack_ack;
+
+ ubxLastAck = *ack_ack;
+}
+
+static void parse_ubx_ack_nak(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
+{
+ struct UBX_ACK_NAK *ack_nak = &ubx->payload.ack_nak;
+
+ ubxLastNak = *ack_nak;
+}
+
+static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
+{
+ struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver;
+
+ ubxHwVersion = atoi(mon_ver->hwVersion);
+
+ sensorType = (ubxHwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 :
+ ((ubxHwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX);
+}
+
+
+#if !defined(PIOS_GPS_MINIMAL)
+static void parse_ubx_op_sys(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
+{
+ struct UBX_OP_SYSINFO *sysinfo = &ubx->payload.op_sysinfo;
+ GPSExtendedStatusData data;
+
+ data.FlightTime = sysinfo->flightTime;
+ data.HeapRemaining = sysinfo->HeapRemaining;
+ data.IRQStackRemaining = sysinfo->IRQStackRemaining;
+ data.SysModStackRemaining = sysinfo->SystemModStackRemaining;
+ data.Options = sysinfo->options;
+ data.Status = GPSEXTENDEDSTATUS_STATUS_GPSV9;
+ GPSExtendedStatusSet(&data);
+}
+#endif
+static void parse_ubx_op_mag(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
+{
+ if (!useMag) {
+ return;
+ }
+ struct UBX_OP_MAG *mag = &ubx->payload.op_mag;
+ float mags[3] = { mag->x, mag->y, mag->z };
+ auxmagsupport_publish_samples(mags, AUXMAGSENSOR_STATUS_OK);
+}
+
+
// UBX message parser
// returns UAVObjectID if a UAVObject structure is ready for further processing
uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
uint32_t id = 0;
- static uint32_t lastPvtTime = 0;
- static bool ubxInitialized = false;
+ static bool ubxInitialized = false;
if (!ubxInitialized) {
// initialize dop values. If no DOP sentence is received it is safer to initialize them to a high value rather than 0.
@@ -361,48 +500,17 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
ubxInitialized = true;
}
// is it using PVT?
- bool usePvt = (lastPvtTime) && (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000);
-
- switch (ubx->header.class) {
- case UBX_CLASS_NAV:
- if (!usePvt) {
- // Set of messages to be decoded when not using pvt
- switch (ubx->header.id) {
- case UBX_ID_POSLLH:
- parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition);
- break;
- case UBX_ID_SOL:
- parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition);
- break;
- case UBX_ID_VELNED:
- parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition);
- break;
-#if !defined(PIOS_GPS_MINIMAL)
- case UBX_ID_TIMEUTC:
- parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc);
- break;
-#endif
- }
+ usePvt = (lastPvtTime) && (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000);
+ for (uint8_t i = 0; i < UBX_HANDLER_TABLE_SIZE; i++) {
+ const ubx_message_handler *handler = &ubx_handler_table[i];
+ if (handler->msgClass == ubx->header.class && handler->msgID == ubx->header.id) {
+ handler->handler(ubx, GpsPosition);
+ break;
}
- // messages used always
- switch (ubx->header.id) {
- case UBX_ID_DOP:
- parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition);
- break;
-
- case UBX_ID_PVT:
- parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition);
- lastPvtTime = PIOS_DELAY_GetuS();
- break;
-#if !defined(PIOS_GPS_MINIMAL)
- case UBX_ID_SVINFO:
- parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo);
- break;
-#endif
- }
- break;
}
+ GpsPosition->SensorType = sensorType;
+
if (msgtracker.msg_received == ALL_RECEIVED) {
GPSPositionSensorSet(GpsPosition);
msgtracker.msg_received = NONE_RECEIVED;
@@ -411,4 +519,17 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
return id;
}
+void load_mag_settings()
+{
+ auxmagsupport_reload_settings();
+
+ if (auxmagsupport_get_type() != AUXMAGSETTINGS_TYPE_GPSV9) {
+ useMag = false;
+ const uint8_t status = AUXMAGSENSOR_STATUS_NONE;
+ // next sample from other external mags will provide the right status if present
+ AuxMagSensorStatusSet((uint8_t *)&status);
+ } else {
+ useMag = true;
+ }
+}
#endif // PIOS_INCLUDE_GPS_UBX_PARSER
diff --git a/flight/modules/GPS/inc/GPS.h b/flight/modules/GPS/inc/GPS.h
index 6cecaa845..c952db4cb 100644
--- a/flight/modules/GPS/inc/GPS.h
+++ b/flight/modules/GPS/inc/GPS.h
@@ -38,6 +38,7 @@
#include "gpssatellites.h"
#include "gpspositionsensor.h"
#include "gpstime.h"
+#include "auxmagsettings.h"
#define NO_PARSER -3 // no parser available
#define PARSER_OVERRUN -2 // message buffer overrun before completing the message
diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h
index f32a267bc..1b1256e1f 100644
--- a/flight/modules/GPS/inc/UBX.h
+++ b/flight/modules/GPS/inc/UBX.h
@@ -32,27 +32,99 @@
#define UBX_H
#include "openpilot.h"
#include "gpspositionsensor.h"
+#include "gpsextendedstatus.h"
+#include "auxmagsensor.h"
#include "GPS.h"
+#define UBX_HW_VERSION_8 80000
+#define UBX_HW_VERSION_7 70000
-#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters
-#define UBX_SYNC2 0x62
+#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters
+#define UBX_SYNC2 0x62
// From u-blox6 receiver protocol specification
// Messages classes
-#define UBX_CLASS_NAV 0x01
+typedef enum {
+ UBX_CLASS_NAV = 0x01,
+ UBX_CLASS_ACK = 0x05,
+ UBX_CLASS_CFG = 0x06,
+ UBX_CLASS_MON = 0x0A,
+ UBX_CLASS_OP_CUST = 0x99,
+ UBX_CLASS_AID = 0x0B,
+ // unused class IDs, used for disabling them
+ UBX_CLASS_RXM = 0x02,
+} ubx_class;
// Message IDs
-#define UBX_ID_POSLLH 0x02
-#define UBX_ID_STATUS 0x03
-#define UBX_ID_DOP 0x04
-#define UBX_ID_SOL 0x06
-#define UBX_ID_VELNED 0x12
-#define UBX_ID_TIMEUTC 0x21
-#define UBX_ID_SVINFO 0x30
-#define UBX_ID_PVT 0x07
+typedef enum {
+ UBX_ID_NAV_POSLLH = 0x02,
+ UBX_ID_NAV_STATUS = 0x03,
+ UBX_ID_NAV_DOP = 0x04,
+ UBX_ID_NAV_SOL = 0x06,
+ UBX_ID_NAV_VELNED = 0x12,
+ UBX_ID_NAV_TIMEUTC = 0x21,
+ UBX_ID_NAV_SVINFO = 0x30,
+ UBX_ID_NAV_PVT = 0x07,
+ UBX_ID_NAV_AOPSTATUS = 0x60,
+ UBX_ID_NAV_CLOCK = 0x22,
+ UBX_ID_NAV_DGPS = 0x31,
+ UBX_ID_NAV_POSECEF = 0x01,
+ UBX_ID_NAV_SBAS = 0x32,
+ UBX_ID_NAV_TIMEGPS = 0x20,
+ UBX_ID_NAV_VELECEF = 0x11
+} ubx_class_nav_id;
+
+typedef enum {
+ UBX_ID_OP_SYS = 0x01,
+ UBX_ID_OP_MAG = 0x02,
+} ubx_class_op_id;
+
+typedef enum {
+ UBX_ID_MON_VER = 0x04,
+ // unused messages IDs, used for disabling them
+ UBX_ID_MON_HW2 = 0x0B,
+ UBX_ID_MON_HW = 0x09,
+ UBX_ID_MON_IO = 0x02,
+ UBX_ID_MON_MSGPP = 0x06,
+ UBX_ID_MON_RXBUFF = 0x07,
+ UBX_ID_MON_RXR = 0x21,
+ UBX_ID_MON_TXBUF = 0x08,
+} ubx_class_mon_id;
+
+typedef enum {
+ UBX_ID_CFG_NAV5 = 0x24,
+ UBX_ID_CFG_RATE = 0x08,
+ UBX_ID_CFG_MSG = 0x01,
+ UBX_ID_CFG_CFG = 0x09,
+ UBX_ID_CFG_SBAS = 0x16,
+} ubx_class_cfg_id;
+
+typedef enum {
+ UBX_ID_ACK_ACK = 0x01,
+ UBX_ID_ACK_NAK = 0x00,
+} ubx_class_ack_id;
+
+typedef enum {
+ UBX_ID_AID_ALM = 0x0B,
+ UBX_ID_AID_ALPSRV = 0x32,
+ UBX_ID_AID_ALP = 0x50,
+ UBX_ID_AID_AOP = 0x33,
+ UBX_ID_AID_DATA = 0x10,
+ UBX_ID_AID_EPH = 0x31,
+ UBX_ID_AID_HUI = 0x02,
+ UBX_ID_AID_INI = 0x01,
+ UBX_ID_AID_REQ = 0x00,
+} ubx_class_aid_id;
+
+typedef enum {
+ UBX_ID_RXM_ALM = 0x30,
+ UBX_ID_RXM_EPH = 0x31,
+ UBX_ID_RXM_RAW = 0x10,
+ UBX_ID_RXM_SFRB = 0x11,
+ UBX_ID_RXM_SVSI = 0x20,
+} ubx_class_rxm_id;
// private structures
// Geodetic Position Solution
@@ -245,8 +317,49 @@ struct UBX_NAV_SVINFO {
struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times
};
+// ACK message class
+
+struct UBX_ACK_ACK {
+ uint8_t clsID; // ClassID
+ uint8_t msgID; // MessageID
+};
+
+struct UBX_ACK_NAK {
+ uint8_t clsID; // ClassID
+ uint8_t msgID; // MessageID
+};
+
+// MON message Class
+#define UBX_MON_MAX_EXT 5
+struct UBX_MON_VER {
+ char swVersion[30];
+ char hwVersion[10];
+#if UBX_MON_MAX_EXT > 0
+ char extension[UBX_MON_MAX_EXT][30];
+#endif
+};
+
+
+// OP custom messages
+struct UBX_OP_SYSINFO {
+ uint32_t flightTime;
+ uint16_t HeapRemaining;
+ uint16_t IRQStackRemaining;
+ uint16_t SystemModStackRemaining;
+ uint16_t options;
+};
+
+// OP custom messages
+struct UBX_OP_MAG {
+ int16_t x;
+ int16_t y;
+ int16_t z;
+ uint16_t Status;
+};
+
typedef union {
uint8_t payload[0];
+ // Nav Class
struct UBX_NAV_POSLLH nav_posllh;
struct UBX_NAV_STATUS nav_status;
struct UBX_NAV_DOP nav_dop;
@@ -257,6 +370,13 @@ typedef union {
struct UBX_NAV_TIMEUTC nav_timeutc;
struct UBX_NAV_SVINFO nav_svinfo;
#endif
+ // Ack Class
+ struct UBX_ACK_ACK ack_ack;
+ struct UBX_ACK_NAK ack_nak;
+ // Mon Class
+ struct UBX_MON_VER mon_ver;
+ struct UBX_OP_SYSINFO op_sysinfo;
+ struct UBX_OP_MAG op_mag;
} UBXPayload;
struct UBXHeader {
@@ -272,8 +392,15 @@ struct UBXPacket {
UBXPayload payload;
};
+// Used by AutoConfig code
+extern int32_t ubxHwVersion;
+extern struct UBX_ACK_ACK ubxLastAck;
+extern struct UBX_ACK_NAK ubxLastNak;
+
bool checksum_ubx_message(struct UBXPacket *);
uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
+
int parse_ubx_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
+void load_mag_settings();
#endif /* UBX_H */
diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h
new file mode 100644
index 000000000..e01391a7d
--- /dev/null
+++ b/flight/modules/GPS/inc/ubx_autoconfig.h
@@ -0,0 +1,185 @@
+/**
+ ******************************************************************************
+ *
+ * @file %FILENAME%
+ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @addtogroup [Group]
+ * @{
+ * @addtogroup %CLASS%
+ * @{
+ * @brief [Brief]
+ *****************************************************************************/
+/*
+ * 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 UBX_AUTOCONFIG_H_
+#define UBX_AUTOCONFIG_H_
+#include "UBX.h"
+#include <stdint.h>
+#include <stdbool.h>
+
+// defines
+// TODO: NEO8 max rate is for Rom version, flash is limited to 10Hz, need to handle that.
+#define UBX_MAX_RATE_VER8 18
+#define UBX_MAX_RATE_VER7 10
+#define UBX_MAX_RATE 5
+
+#define UBX_REPLY_TIMEOUT (500 * 1000)
+#define UBX_MAX_RETRIES 5
+
+// types
+typedef enum {
+ UBX_AUTOCONFIG_STATUS_DISABLED = 0,
+ UBX_AUTOCONFIG_STATUS_RUNNING,
+ UBX_AUTOCONFIG_STATUS_DONE,
+ UBX_AUTOCONFIG_STATUS_ERROR
+} ubx_autoconfig_status_t;
+// Enumeration options for field UBXDynamicModel
+typedef enum {
+ UBX_DYNMODEL_PORTABLE = 0,
+ UBX_DYNMODEL_STATIONARY = 2,
+ UBX_DYNMODEL_PEDESTRIAN = 3,
+ UBX_DYNMODEL_AUTOMOTIVE = 4,
+ UBX_DYNMODEL_SEA = 5,
+ UBX_DYNMODEL_AIRBORNE1G = 6,
+ UBX_DYNMODEL_AIRBORNE2G = 7,
+ UBX_DYNMODEL_AIRBORNE4G = 8
+} ubx_config_dynamicmodel_t;
+
+typedef enum {
+ UBX_SBAS_SATS_AUTOSCAN = 0,
+ UBX_SBAS_SATS_WAAS = 1,
+ UBX_SBAS_SATS_EGNOS = 2,
+ UBX_SBAS_SATS_MSAS = 3,
+ UBX_SBAS_SATS_GAGAN = 4,
+ UBX_SBAS_SATS_SDCM = 5
+} ubx_config_sats_t;
+
+#define UBX_
+typedef struct {
+ bool autoconfigEnabled;
+ bool storeSettings;
+
+ bool SBASRanging;
+ bool SBASCorrection;
+ bool SBASIntegrity;
+ ubx_config_sats_t SBASSats;
+ uint8_t SBASChannelsUsed;
+
+ int8_t navRate;
+ ubx_config_dynamicmodel_t dynamicModel;
+} ubx_autoconfig_settings_t;
+
+// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash
+#define UBX_CFG_CFG_ALL_DEVICES_MASK (0x01 | 0x02 | 0x04 | 0x10)
+
+// Sent messages for configuration support
+typedef struct {
+ uint32_t clearMask;
+ uint32_t saveMask;
+ uint32_t loadMask;
+ uint8_t deviceMask;
+} __attribute__((packed)) ubx_cfg_cfg_t;
+
+typedef struct {
+ uint16_t mask;
+ uint8_t dynModel;
+ uint8_t fixMode;
+ int32_t fixedAlt;
+ uint32_t fixedAltVar;
+ int8_t minElev;
+ uint8_t drLimit;
+ uint16_t pDop;
+ uint16_t tDop;
+ uint16_t pAcc;
+ uint16_t tAcc;
+ uint8_t staticHoldThresh;
+ uint8_t dgpsTimeOut;
+ uint8_t cnoThreshNumSVs;
+ uint8_t cnoThresh;
+ uint16_t reserved2;
+ uint32_t reserved3;
+ uint32_t reserved4;
+} __attribute__((packed)) ubx_cfg_nav5_t;
+
+typedef struct {
+ uint16_t measRate;
+ uint16_t navRate;
+ uint16_t timeRef;
+} __attribute__((packed)) ubx_cfg_rate_t;
+
+typedef struct {
+ uint8_t msgClass;
+ uint8_t msgID;
+ uint8_t rate;
+} __attribute__((packed)) ubx_cfg_msg_t;
+
+#define UBX_CFG_SBAS_MODE_ENABLED 0x01
+#define UBX_CFG_SBAS_MODE_TEST 0x02
+#define UBX_CFG_SBAS_USAGE_RANGE 0x01
+#define UBX_CFG_SBAS_USAGE_DIFFCORR 0x02
+#define UBX_CFG_SBAS_USAGE_INTEGRITY 0x04
+
+// SBAS used satellite PNR bitmask (120-151)
+// -------------------------------------1---------1---------1---------1
+// -------------------------------------5---------4---------3---------2
+// ------------------------------------10987654321098765432109876543210
+// WAAS 122, 133, 134, 135, 138---------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_WAAS 0b00000000000001001110000000000100
+// EGNOS 120, 124, 126, 131-------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001
+// MSAS 129, 137------------------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_MSAS 0b00000000000000100000001000000000
+// GAGAN 127, 128-----------------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_GAGAN 0b00000000000000000000000110000000
+// SDCM 125, 140, 141-------------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_SDCM 0b00000000001100000000000000100000
+
+#define UBX_CFG_SBAS_SCANMODE2 0x00
+typedef struct {
+ uint8_t mode;
+ uint8_t usage;
+ uint8_t maxSBAS;
+ uint8_t scanmode2;
+ uint32_t scanmode1;
+} __attribute__((packed)) ubx_cfg_sbas_t;
+
+typedef struct {
+ uint8_t prolog[2];
+ uint8_t class;
+ uint8_t id;
+ uint16_t len;
+} __attribute__((packed)) UBXSentHeader_t;
+
+typedef union {
+ uint8_t buffer[0];
+ struct {
+ UBXSentHeader_t header;
+ union {
+ ubx_cfg_cfg_t cfg_cfg;
+ ubx_cfg_msg_t cfg_msg;
+ ubx_cfg_nav5_t cfg_nav5;
+ ubx_cfg_rate_t cfg_rate;
+ ubx_cfg_sbas_t cfg_sbas;
+ } payload;
+ uint8_t resvd[2]; // added space for checksum bytes
+ } message;
+} __attribute__((packed)) UBXSentPacket_t;
+
+void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send);
+void ubx_autoconfig_set(ubx_autoconfig_settings_t config);
+int32_t ubx_autoconfig_get_status();
+#endif /* UBX_AUTOCONFIG_H_ */
diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c
new file mode 100644
index 000000000..74aa89179
--- /dev/null
+++ b/flight/modules/GPS/ubx_autoconfig.c
@@ -0,0 +1,399 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup GSPModule GPS Module
+ * @brief Support code for UBX AutoConfig
+ * @{
+ *
+ * @file ubx_autoconfig.c
+ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief Support code for UBX AutoConfig
+ * @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 "inc/ubx_autoconfig.h"
+#include <pios_mem.h>
+// private type definitions
+typedef enum {
+ INIT_STEP_DISABLED = 0,
+ INIT_STEP_START,
+ INIT_STEP_ASK_VER,
+ INIT_STEP_WAIT_VER,
+ INIT_STEP_ENABLE_SENTENCES,
+ INIT_STEP_ENABLE_SENTENCES_WAIT_ACK,
+ INIT_STEP_CONFIGURE,
+ INIT_STEP_CONFIGURE_WAIT_ACK,
+ INIT_STEP_DONE,
+ INIT_STEP_ERROR,
+} initSteps_t;
+
+typedef struct {
+ initSteps_t currentStep; // Current configuration "fsm" status
+ uint32_t lastStepTimestampRaw; // timestamp of last operation
+ UBXSentPacket_t working_packet; // outbound "buffer"
+ ubx_autoconfig_settings_t currentSettings;
+ int8_t lastConfigSent; // index of last configuration string sent
+ struct UBX_ACK_ACK requiredAck; // Class and id of the message we are waiting for an ACK from GPS
+ uint8_t retryCount;
+} status_t;
+
+ubx_cfg_msg_t msg_config_ubx6[] = {
+ // messages to disable
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_CLOCK, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSECEF, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SBAS, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEGPS, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELECEF, .rate = 0 },
+
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW, .rate = 0 },
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW2, .rate = 0 },
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_IO, .rate = 0 },
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_MSGPP, .rate = 0 },
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_RXBUFF, .rate = 0 },
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_RXR, .rate = 0 },
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_TXBUF, .rate = 0 },
+
+ { .msgClass = UBX_CLASS_RXM, .msgID = UBX_ID_RXM_SVSI, .rate = 0 },
+
+ // message to enable
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .rate = 1 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .rate = 1 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL, .rate = 1 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_STATUS, .rate = 1 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED, .rate = 1 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEUTC, .rate = 1 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .rate = 10 },
+};
+
+ubx_cfg_msg_t msg_config_ubx7[] = {
+ // messages to disable
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_AOPSTATUS, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_CLOCK, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DGPS, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSECEF, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SBAS, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEGPS, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELECEF, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_STATUS, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEUTC, .rate = 0 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .rate = 0 },
+
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW, .rate = 0 },
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW2, .rate = 0 },
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_IO, .rate = 0 },
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_MSGPP, .rate = 0 },
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_RXBUFF, .rate = 0 },
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_RXR, .rate = 0 },
+ { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_TXBUF, .rate = 0 },
+
+ { .msgClass = UBX_CLASS_RXM, .msgID = UBX_ID_RXM_SVSI, .rate = 0 },
+
+ // message to enable
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_PVT, .rate = 1 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .rate = 1 },
+ { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .rate = 10 },
+};
+
+// private defines
+#define LAST_CONFIG_SENT_START (-1)
+#define LAST_CONFIG_SENT_COMPLETED (-2)
+
+// private variables
+
+// enable the autoconfiguration system
+static bool enabled;
+
+static status_t *status = 0;
+
+static void append_checksum(UBXSentPacket_t *packet)
+{
+ uint8_t i;
+ uint8_t ck_a = 0;
+ uint8_t ck_b = 0;
+ uint16_t len = packet->message.header.len + sizeof(UBXSentHeader_t);
+
+ for (i = 2; i < len; i++) {
+ ck_a += packet->buffer[i];
+ ck_b += ck_a;
+ }
+
+ packet->buffer[len] = ck_a;
+ packet->buffer[len + 1] = ck_b;
+}
+/**
+ * prepare a packet to be sent, fill the header and appends the checksum.
+ * return the total packet lenght comprising header and checksum
+ */
+static uint16_t prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t len)
+{
+ packet->message.header.prolog[0] = UBX_SYNC1;
+ packet->message.header.prolog[1] = UBX_SYNC2;
+ packet->message.header.class = classID;
+ packet->message.header.id = messageID;
+ packet->message.header.len = len;
+ append_checksum(packet);
+ return packet->message.header.len + sizeof(UBXSentHeader_t) + 2; // header + payload + checksum
+}
+
+static void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *bytes_to_send)
+{
+ *bytes_to_send = prepare_packet(packet, classID, messageID, 0);
+}
+
+void config_rate(uint16_t *bytes_to_send)
+{
+ memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t));
+ // if rate is less than 1 uses the highest rate for current hardware
+ uint16_t rate = status->currentSettings.navRate > 0 ? status->currentSettings.navRate : 99;
+ if (ubxHwVersion < UBX_HW_VERSION_7 && rate > UBX_MAX_RATE) {
+ rate = UBX_MAX_RATE;
+ } else if (ubxHwVersion < UBX_HW_VERSION_8 && rate > UBX_MAX_RATE_VER7) {
+ rate = UBX_MAX_RATE_VER7;
+ } else if (ubxHwVersion >= UBX_HW_VERSION_8 && rate > UBX_MAX_RATE_VER8) {
+ rate = UBX_MAX_RATE_VER8;
+ }
+ uint16_t period = 1000 / rate;
+
+ status->working_packet.message.payload.cfg_rate.measRate = period;
+ status->working_packet.message.payload.cfg_rate.navRate = 1; // must be set to 1
+ status->working_packet.message.payload.cfg_rate.timeRef = 1; // 0 = UTC Time, 1 = GPS Time
+ *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t));
+ status->requiredAck.clsID = UBX_CLASS_CFG;
+ status->requiredAck.msgID = UBX_ID_CFG_RATE;
+}
+
+void config_nav(uint16_t *bytes_to_send)
+{
+ memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t));
+
+ status->working_packet.message.payload.cfg_nav5.dynModel = status->currentSettings.dynamicModel;
+ status->working_packet.message.payload.cfg_nav5.fixMode = 2; // 1=2D only, 2=3D only, 3=Auto 2D/3D
+ // mask LSB=dyn|minEl|posFixMode|drLim|posMask|statisticHoldMask|dgpsMask|......|reservedBit0 = MSB
+
+ status->working_packet.message.payload.cfg_nav5.mask = 0x01 + 0x04; // Dyn Model | posFixMode configuration
+ *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t));
+ status->requiredAck.clsID = UBX_CLASS_CFG;
+ status->requiredAck.msgID = UBX_ID_CFG_NAV5;
+}
+
+void config_sbas(uint16_t *bytes_to_send)
+{
+ memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_sbas_t));
+
+ status->working_packet.message.payload.cfg_sbas.maxSBAS = status->currentSettings.SBASChannelsUsed < 4 ?
+ status->currentSettings.SBASChannelsUsed : 3;
+
+ status->working_packet.message.payload.cfg_sbas.usage =
+ (status->currentSettings.SBASCorrection ? UBX_CFG_SBAS_USAGE_DIFFCORR : 0) |
+ (status->currentSettings.SBASIntegrity ? UBX_CFG_SBAS_USAGE_INTEGRITY : 0) |
+ (status->currentSettings.SBASRanging ? UBX_CFG_SBAS_USAGE_RANGE : 0);
+ // If sbas is used for anything then set mode as enabled
+ status->working_packet.message.payload.cfg_sbas.mode =
+ status->working_packet.message.payload.cfg_sbas.usage != 0 ? UBX_CFG_SBAS_MODE_ENABLED : 0;
+
+ status->working_packet.message.payload.cfg_sbas.scanmode1 =
+ status->currentSettings.SBASSats == UBX_SBAS_SATS_WAAS ? UBX_CFG_SBAS_SCANMODE1_WAAS :
+ status->currentSettings.SBASSats == UBX_SBAS_SATS_EGNOS ? UBX_CFG_SBAS_SCANMODE1_EGNOS :
+ status->currentSettings.SBASSats == UBX_SBAS_SATS_MSAS ? UBX_CFG_SBAS_SCANMODE1_MSAS :
+ status->currentSettings.SBASSats == UBX_SBAS_SATS_GAGAN ? UBX_CFG_SBAS_SCANMODE1_GAGAN :
+ status->currentSettings.SBASSats == UBX_SBAS_SATS_SDCM ? UBX_CFG_SBAS_SCANMODE1_SDCM : UBX_SBAS_SATS_AUTOSCAN;
+
+ status->working_packet.message.payload.cfg_sbas.scanmode2 = UBX_CFG_SBAS_SCANMODE2;
+
+ *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t));
+ status->requiredAck.clsID = UBX_CLASS_CFG;
+ status->requiredAck.msgID = UBX_ID_CFG_SBAS;
+}
+
+void config_save(uint16_t *bytes_to_send)
+{
+ memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
+ // mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB
+ status->working_packet.message.payload.cfg_cfg.saveMask = 0x01 | 0x08; // msgConf + navConf
+ status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_ALL_DEVICES_MASK;
+ *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
+ status->requiredAck.clsID = UBX_CLASS_CFG;
+ status->requiredAck.msgID = UBX_ID_CFG_CFG;
+}
+static void configure(uint16_t *bytes_to_send)
+{
+ switch (status->lastConfigSent) {
+ case LAST_CONFIG_SENT_START:
+ config_rate(bytes_to_send);
+ break;
+ case LAST_CONFIG_SENT_START + 1:
+ config_nav(bytes_to_send);
+ break;
+ case LAST_CONFIG_SENT_START + 2:
+ config_sbas(bytes_to_send);
+ if (!status->currentSettings.storeSettings) {
+ // skips saving
+ status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
+ }
+ break;
+ case LAST_CONFIG_SENT_START + 3:
+ config_save(bytes_to_send);
+ status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
+ return;
+
+ default:
+ status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
+ return;
+ }
+}
+
+static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
+{
+ int8_t msg = status->lastConfigSent + 1;
+ uint8_t msg_count = (ubxHwVersion >= UBX_HW_VERSION_7) ?
+ NELEMENTS(msg_config_ubx7) : NELEMENTS(msg_config_ubx6);
+ ubx_cfg_msg_t *msg_config = (ubxHwVersion >= UBX_HW_VERSION_7) ?
+ &msg_config_ubx7[0] : &msg_config_ubx6[0];
+
+ if (msg >= 0 && msg < msg_count) {
+ status->working_packet.message.payload.cfg_msg = msg_config[msg];
+
+ *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t));
+ status->requiredAck.clsID = UBX_CLASS_CFG;
+ status->requiredAck.msgID = UBX_ID_CFG_MSG;
+ } else {
+ status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
+ }
+}
+
+void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
+{
+ *bytes_to_send = 0;
+ *buffer = (char *)status->working_packet.buffer;
+ if (!status || !enabled) {
+ return; // autoconfig not enabled
+ }
+ switch (status->currentStep) {
+ case INIT_STEP_ERROR: // TODO: what to do? retries after a while? maybe gps was disconnected (this can be detected)?
+ case INIT_STEP_DISABLED:
+ case INIT_STEP_DONE:
+ return;
+
+ case INIT_STEP_START:
+ case INIT_STEP_ASK_VER:
+ status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+ build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
+ status->currentStep = INIT_STEP_WAIT_VER;
+ break;
+
+ case INIT_STEP_WAIT_VER:
+ if (ubxHwVersion > 0) {
+ status->lastConfigSent = LAST_CONFIG_SENT_START;
+ status->currentStep = INIT_STEP_ENABLE_SENTENCES;
+ status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+ return;
+ }
+
+ if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) {
+ status->currentStep = INIT_STEP_ASK_VER;
+ }
+ return;
+
+ case INIT_STEP_ENABLE_SENTENCES:
+ case INIT_STEP_CONFIGURE:
+ {
+ bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE);
+ if (step_configure) {
+ configure(bytes_to_send);
+ } else {
+ enable_sentences(bytes_to_send);
+ }
+
+ status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+ if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) {
+ status->lastConfigSent = LAST_CONFIG_SENT_START;
+ status->currentStep = step_configure ? INIT_STEP_DONE : INIT_STEP_CONFIGURE;
+ } else {
+ status->currentStep = step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK;
+ }
+ return;
+ }
+ case INIT_STEP_ENABLE_SENTENCES_WAIT_ACK:
+ case INIT_STEP_CONFIGURE_WAIT_ACK: // Wait for an ack from GPS
+ {
+ bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE_WAIT_ACK);
+ if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
+ // clear ack
+ ubxLastAck.clsID = 0x00;
+ ubxLastAck.msgID = 0x00;
+
+ // Continue with next configuration option
+ status->retryCount = 0;
+ status->lastConfigSent++;
+ } else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) {
+ // timeout, resend the message or abort
+ status->retryCount++;
+ if (status->retryCount > UBX_MAX_RETRIES) {
+ status->currentStep = INIT_STEP_ERROR;
+ return;
+ }
+ }
+ // no abort condition, continue or retries.,
+ if (step_configure) {
+ status->currentStep = INIT_STEP_CONFIGURE;
+ } else {
+ status->currentStep = INIT_STEP_ENABLE_SENTENCES;
+ }
+ return;
+ }
+ }
+}
+
+void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
+{
+ enabled = false;
+ if (config.autoconfigEnabled) {
+ if (!status) {
+ status = (status_t *)pios_malloc(sizeof(status_t));
+ memset(status, 0, sizeof(status_t));
+ status->currentStep = INIT_STEP_DISABLED;
+ }
+ status->currentSettings = config;
+ status->currentStep = INIT_STEP_START;
+ enabled = true;
+ }
+}
+
+int32_t ubx_autoconfig_get_status()
+{
+ if (!status) {
+ return UBX_AUTOCONFIG_STATUS_DISABLED;
+ }
+ switch (status->currentStep) {
+ case INIT_STEP_ERROR:
+ return UBX_AUTOCONFIG_STATUS_ERROR;
+
+ case INIT_STEP_DISABLED:
+ return UBX_AUTOCONFIG_STATUS_DISABLED;
+
+ case INIT_STEP_DONE:
+ return UBX_AUTOCONFIG_STATUS_DONE;
+
+ default:
+ break;
+ }
+ return UBX_AUTOCONFIG_STATUS_RUNNING;
+}
diff --git a/flight/modules/StateEstimation/filterlla.c b/flight/modules/StateEstimation/filterlla.c
index 327fa9a00..352e0000c 100644
--- a/flight/modules/StateEstimation/filterlla.c
+++ b/flight/modules/StateEstimation/filterlla.c
@@ -103,7 +103,7 @@ static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstim
GPSPositionSensorGet(&gpsdata);
// check if we have a valid GPS signal (not checked by StateEstimation istelf)
- if ((gpsdata.PDOP < this->settings.MaxPDOP) && (gpsdata.Satellites >= this->settings.MinSattelites) &&
+ if ((gpsdata.PDOP < this->settings.MaxPDOP) && (gpsdata.Satellites >= this->settings.MinSatellites) &&
(gpsdata.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
(gpsdata.Latitude != 0 || gpsdata.Longitude != 0)) {
int32_t LLAi[3] = {
diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c
index c48c4463e..4fa5afa5c 100644
--- a/flight/modules/StateEstimation/filtermag.c
+++ b/flight/modules/StateEstimation/filtermag.c
@@ -36,8 +36,9 @@
#include <revosettings.h>
#include <systemalarms.h>
#include <homelocation.h>
-
+#include <auxmagsettings.h>
#include <CoordinateConversions.h>
+#include <mathmisc.h>
// Private constants
//
@@ -47,10 +48,12 @@
struct data {
RevoCalibrationData revoCalibration;
RevoSettingsData revoSettings;
+ AuxMagSettingsUsageOptions auxMagUsage;
uint8_t warningcount;
uint8_t errorcount;
float homeLocationBe[3];
- float magBe2;
+ float magBe;
+ float invMagBe;
float magBias[3];
};
@@ -60,9 +63,9 @@ struct data {
static int32_t init(stateFilter *self);
static filterResult filter(stateFilter *self, stateEstimation *state);
-static void checkMagValidity(struct data *this, float mag[3]);
+static bool checkMagValidity(struct data *this, float error, bool setAlarms);
static void magOffsetEstimation(struct data *this, float mag[3]);
-
+static float getMagError(struct data *this, float mag[3]);
int32_t filterMagInitialize(stateFilter *handle)
{
@@ -80,80 +83,132 @@ static int32_t init(stateFilter *self)
this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f;
this->warningcount = this->errorcount = 0;
HomeLocationBeGet(this->homeLocationBe);
- // magBe2 holds the squared magnetic vector length (extpected)
- this->magBe2 = this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1] + this->homeLocationBe[2] * this->homeLocationBe[2];
+ // magBe holds the magnetic vector length (expected)
+ this->magBe = vector_lengthf(this->homeLocationBe, 3);
+ this->invMagBe = 1.0f / this->magBe;
RevoCalibrationGet(&this->revoCalibration);
RevoSettingsGet(&this->revoSettings);
+ AuxMagSettingsUsageGet(&this->auxMagUsage);
return 0;
}
static filterResult filter(stateFilter *self, stateEstimation *state)
{
- struct data *this = (struct data *)self->localdata;
+ struct data *this = (struct data *)self->localdata;
+ float auxMagError;
+ float boardMagError;
+ float temp_mag[3] = { 0 };
+ uint8_t temp_status = MAGSTATUS_INVALID;
+ uint8_t magSamples = 0;
- if (IS_SET(state->updated, SENSORUPDATES_mag)) {
- checkMagValidity(this, state->mag);
- if (this->revoCalibration.MagBiasNullingRate > 0) {
- magOffsetEstimation(this, state->mag);
+ // Uses the external mag when available
+ if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_ONBOARDONLY) &&
+ IS_SET(state->updated, SENSORUPDATES_auxMag)) {
+ auxMagError = getMagError(this, state->auxMag);
+ // Handles alarms only if it will rely on aux mag only
+ bool auxMagValid = checkMagValidity(this, auxMagError, (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY));
+ // if we are going to use Aux only, force the update even if mag is invalid
+ if (auxMagValid || (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY)) {
+ temp_mag[0] = state->auxMag[0];
+ temp_mag[1] = state->auxMag[1];
+ temp_mag[2] = state->auxMag[2];
+ temp_status = MAGSTATUS_AUX;
+ magSamples++;
}
}
+ if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_AUXONLY) &&
+ IS_SET(state->updated, SENSORUPDATES_boardMag)) {
+ // TODO:mag Offset estimation works with onboard mag only right now.
+ if (this->revoCalibration.MagBiasNullingRate > 0) {
+ magOffsetEstimation(this, state->boardMag);
+ }
+ boardMagError = getMagError(this, state->boardMag);
+ // sets warning only if no mag data are available (aux is invalid or missing)
+ bool boardMagValid = checkMagValidity(this, boardMagError, (temp_status == MAGSTATUS_INVALID));
+ // force it to be set to board mag value if no data has been feed to temp_mag yet.
+ // this works also as a failsafe in case aux mag stops feeding data.
+ if (boardMagValid || (temp_status == MAGSTATUS_INVALID)) {
+ temp_mag[0] += state->boardMag[0];
+ temp_mag[1] += state->boardMag[1];
+ temp_mag[2] += state->boardMag[2];
+ temp_status = MAGSTATUS_OK;
+ magSamples++;
+ }
+ }
+
+ if (magSamples > 1) {
+ temp_mag[0] *= 0.5f;
+ temp_mag[1] *= 0.5f;
+ temp_mag[2] *= 0.5f;
+ }
+
+ if (temp_status != MAGSTATUS_INVALID) {
+ state->mag[0] = temp_mag[0];
+ state->mag[1] = temp_mag[1];
+ state->mag[2] = temp_mag[2];
+ state->updated |= SENSORUPDATES_mag;
+ }
+ state->magStatus = temp_status;
return FILTERRESULT_OK;
}
/**
* check validity of magnetometers
*/
-static void checkMagValidity(struct data *this, float mag[3])
+static bool checkMagValidity(struct data *this, float error, bool setAlarms)
{
- #define ALARM_THRESHOLD 5
-
- // mag2 holds the actual magnetic vector length (squared)
- float mag2 = mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2];
-
- // warning and error thresholds
- // avoud sqrt() : minlimit<actual<maxlimit === minlimit²<actual²<maxlimit²
- //
- // actual = |mag|
- // minlimit = |Be| - maxDeviation*|Be| = |Be| * (1 - maxDeviation)
- // maxlimit = |Be| + maxDeviation*|Be| = |Be| * (1 + maxDeviation)
- // minlimit² = |Be|² * ( 1 - 2*maxDeviation + maxDeviation²)
- // maxlimit² = |Be|² * ( 1 + 2*maxDeviation + maxDeviation²)
- //
-
- float minWarning2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
- float maxWarning2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
- float minError2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
- float maxError2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
+ #define ALARM_THRESHOLD 5
// set errors
- if (minWarning2 < mag2 && mag2 < maxWarning2) {
+ if (error < this->revoSettings.MagnetometerMaxDeviation.Warning) {
this->warningcount = 0;
this->errorcount = 0;
AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
- } else if (minError2 < mag2 && mag2 < maxError2) {
+ return true;
+ }
+
+ if (error < this->revoSettings.MagnetometerMaxDeviation.Error) {
this->errorcount = 0;
if (this->warningcount > ALARM_THRESHOLD) {
- AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
+ if (setAlarms) {
+ AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
+ }
+ return false;
} else {
this->warningcount++;
- }
- } else {
- if (this->errorcount > ALARM_THRESHOLD) {
- AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
- } else {
- this->errorcount++;
+ return true;
}
}
+
+ if (this->errorcount > ALARM_THRESHOLD) {
+ if (setAlarms) {
+ AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
+ }
+ return false;
+ } else {
+ this->errorcount++;
+ }
+ // still in "grace period"
+ return true;
}
+static float getMagError(struct data *this, float mag[3])
+{
+ // vector norm
+ float magnitude = vector_lengthf(mag, 3);
+ // absolute value of relative error against Be
+ float error = fabsf(magnitude - this->magBe) * this->invMagBe;
+
+ return error;
+}
/**
* Perform an update of the @ref MagBias based on
* Magmeter Offset Cancellation: Theory and Implementation,
* revisited William Premerlani, October 14, 2011
*/
-static void magOffsetEstimation(struct data *this, float mag[3])
+void magOffsetEstimation(struct data *this, float mag[3])
{
#if 0
// Constants, to possibly go into a UAVO
@@ -244,7 +299,6 @@ static void magOffsetEstimation(struct data *this, float mag[3])
#endif // if 0
}
-
/**
* @}
* @}
diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h
index d93235f4e..fd399c818 100644
--- a/flight/modules/StateEstimation/inc/stateestimation.h
+++ b/flight/modules/StateEstimation/inc/stateestimation.h
@@ -47,6 +47,8 @@ typedef enum {
SENSORUPDATES_gyro = 1 << 0,
SENSORUPDATES_accel = 1 << 1,
SENSORUPDATES_mag = 1 << 2,
+ SENSORUPDATES_boardMag = 1 << 10,
+ SENSORUPDATES_auxMag = 1 << 9,
SENSORUPDATES_attitude = 1 << 3,
SENSORUPDATES_pos = 1 << 4,
SENSORUPDATES_vel = 1 << 5,
@@ -55,15 +57,21 @@ typedef enum {
SENSORUPDATES_lla = 1 << 8,
} sensorUpdates;
+#define MAGSTATUS_OK 1
+#define MAGSTATUS_AUX 2
+#define MAGSTATUS_INVALID 0
typedef struct {
- float gyro[3];
- float accel[3];
- float mag[3];
- float attitude[4];
- float pos[3];
- float vel[3];
- float airspeed[2];
- float baro[1];
+ float gyro[3];
+ float accel[3];
+ float mag[3];
+ float attitude[4];
+ float pos[3];
+ float vel[3];
+ float airspeed[2];
+ float baro[1];
+ float auxMag[3];
+ uint8_t magStatus;
+ float boardMag[3];
sensorUpdates updated;
} stateEstimation;
diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c
index 079db8ca1..cf33bbbc4 100644
--- a/flight/modules/StateEstimation/stateestimation.c
+++ b/flight/modules/StateEstimation/stateestimation.c
@@ -41,6 +41,7 @@
#include <gpspositionsensor.h>
#include <gpsvelocitysensor.h>
#include <homelocation.h>
+#include <auxmagsensor.h>
#include <gyrostate.h>
#include <accelstate.h>
@@ -161,14 +162,17 @@ static float gyroDelta[3];
// preconfigured filter chains selectable via revoSettings.FusionAlgorithm
static const filterPipeline *cfQueue = &(filterPipeline) {
- .filter = &airFilter,
+ .filter = &magFilter,
.next = &(filterPipeline) {
- .filter = &baroiFilter,
+ .filter = &airFilter,
.next = &(filterPipeline) {
- .filter = &altitudeFilter,
+ .filter = &baroiFilter,
.next = &(filterPipeline) {
- .filter = &cfFilter,
- .next = NULL,
+ .filter = &altitudeFilter,
+ .next = &(filterPipeline) {
+ .filter = &cfFilter,
+ .next = NULL,
+ }
}
}
}
@@ -267,6 +271,7 @@ int32_t StateEstimationInitialize(void)
GyroSensorInitialize();
MagSensorInitialize();
+ AuxMagSensorInitialize();
BaroSensorInitialize();
AirspeedSensorInitialize();
GPSVelocitySensorInitialize();
@@ -290,6 +295,7 @@ int32_t StateEstimationInitialize(void)
MagSensorConnectCallback(&sensorUpdatedCb);
BaroSensorConnectCallback(&sensorUpdatedCb);
AirspeedSensorConnectCallback(&sensorUpdatedCb);
+ AuxMagSensorConnectCallback(&sensorUpdatedCb);
GPSVelocitySensorConnectCallback(&sensorUpdatedCb);
GPSPositionSensorConnectCallback(&sensorUpdatedCb);
@@ -423,7 +429,8 @@ static void StateEstimationCb(void)
gyroRaw[2] = states.gyro[2];
}
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
- FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z);
+ FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z);
+ FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
@@ -467,7 +474,26 @@ static void StateEstimationCb(void)
gyroDelta[2] = states.gyro[2] - gyroRaw[2];
}
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
- EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(MagState, mag, x, y, z);
+ if (IS_SET(states.updated, SENSORUPDATES_mag)) {
+ MagStateData s;
+
+ MagStateGet(&s);
+ s.x = states.mag[0];
+ s.y = states.mag[1];
+ s.z = states.mag[2];
+ switch (states.magStatus) {
+ case MAGSTATUS_OK:
+ s.Source = MAGSTATE_SOURCE_ONBOARD;
+ break;
+ case MAGSTATUS_AUX:
+ s.Source = MAGSTATE_SOURCE_AUX;
+ break;
+ default:
+ s.Source = MAGSTATE_SOURCE_INVALID;
+ }
+ MagStateSet(&s);
+ }
+
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
@@ -567,7 +593,11 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
}
if (ev->obj == MagSensorHandle()) {
- updatedSensors |= SENSORUPDATES_mag;
+ updatedSensors |= SENSORUPDATES_boardMag;
+ }
+
+ if (ev->obj == AuxMagSensorHandle()) {
+ updatedSensors |= SENSORUPDATES_auxMag;
}
if (ev->obj == GPSPositionSensorHandle()) {
diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile
index 7a5a7e90f..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
@@ -128,6 +129,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/airspeedstate.c
SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c
SRC += $(OPUAVSYNTHDIR)/perfcounter.c
+ SRC += $(OPUAVSYNTHDIR)/auxmagsensor.c
else
## Test Code
SRC += $(OPTESTS)/test_common.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/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
index 49867c39e..60ac23651 100644
--- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
+++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
@@ -31,6 +31,8 @@ UAVOBJSRCFILENAMES += gyrosensor
UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
+UAVOBJSRCFILENAMES += auxmagsensor
+UAVOBJSRCFILENAMES += auxmagsettings
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor
@@ -54,6 +56,7 @@ UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocitysensor
UAVOBJSRCFILENAMES += gpssettings
+UAVOBJSRCFILENAMES += gpsextendedstatus
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile
index 928f1310b..35c8f6e25 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
@@ -98,6 +99,8 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/osdsettings.c
SRC += $(OPUAVSYNTHDIR)/barosensor.c
SRC += $(OPUAVSYNTHDIR)/magsensor.c
+ SRC += $(OPUAVSYNTHDIR)/auxmagsensor.c
+ SRC += $(OPUAVSYNTHDIR)/gpsextendedstatus.c
else
## Test Code
SRC += $(OPTESTS)/test_common.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/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc
index 49867c39e..60ac23651 100644
--- a/flight/targets/boards/revolution/firmware/UAVObjects.inc
+++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc
@@ -31,6 +31,8 @@ UAVOBJSRCFILENAMES += gyrosensor
UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
+UAVOBJSRCFILENAMES += auxmagsensor
+UAVOBJSRCFILENAMES += auxmagsettings
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor
@@ -54,6 +56,7 @@ UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocitysensor
UAVOBJSRCFILENAMES += gpssettings
+UAVOBJSRCFILENAMES += gpsextendedstatus
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c
index b26f75a8c..e029b90dc 100644
--- a/flight/targets/boards/revolution/firmware/pios_board.c
+++ b/flight/targets/boards/revolution/firmware/pios_board.c
@@ -217,6 +217,7 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512
#define PIOS_COM_GPS_RX_BUF_LEN 32
+#define PIOS_COM_GPS_TX_BUF_LEN 32
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65
@@ -609,7 +610,7 @@ void PIOS_Board_Init(void)
PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id);
break;
case HWSETTINGS_RM_MAINPORT_GPS:
- PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id);
+ PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id);
break;
case HWSETTINGS_RM_MAINPORT_SBUS:
#if defined(PIOS_INCLUDE_SBUS)
@@ -699,7 +700,7 @@ void PIOS_Board_Init(void)
#endif /* PIOS_INCLUDE_I2C */
break;
case HWSETTINGS_RM_FLEXIPORT_GPS:
- PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id);
+ PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id);
break;
case HWSETTINGS_RM_FLEXIPORT_DSM2:
case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT:
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
diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc
index 3905af42f..f30d855a2 100644
--- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc
+++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc
@@ -31,6 +31,8 @@ UAVOBJSRCFILENAMES += gyrosensor
UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
+UAVOBJSRCFILENAMES += auxmagsensor
+UAVOBJSRCFILENAMES += auxmagsettings
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor
@@ -54,6 +56,7 @@ UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocitysensor
UAVOBJSRCFILENAMES += gpssettings
+UAVOBJSRCFILENAMES += gpsextendedstatus
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc
index 907ff6833..1498c2e72 100644
--- a/flight/targets/boards/simposix/firmware/UAVObjects.inc
+++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc
@@ -36,6 +36,8 @@ UAVOBJSRCFILENAMES += gyrosensor
UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
+UAVOBJSRCFILENAMES += auxmagsensor
+UAVOBJSRCFILENAMES += auxmagsettings
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor
diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp
index 6a9200d8a..f877a44b5 100644
--- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp
+++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp
@@ -29,7 +29,7 @@
#include "extensionsystem/pluginmanager.h"
#include "calibration/calibrationuiutils.h"
-#include "math.h"
+#include <math.h>
#include <QThread>
#include "QDebug"
@@ -45,6 +45,7 @@ namespace OpenPilot {
SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
QObject(parent),
calibratingMag(false),
+ calibratingAuxMag(false),
calibratingAccel(false),
calibrationStepsMag(),
calibrationStepsAccelOnly(),
@@ -68,6 +69,21 @@ SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW,
tr("Place with left side down, nose south and press Save Position..."));
+ // All mag calibration matrices have the same structure, this is also used when calculating bias/transforms
+ // this is enforced using assert.
+ Q_STATIC_ASSERT((int)RevoCalibration::MAG_TRANSFORM_R0C0 == (int)AuxMagSettings::MAG_TRANSFORM_R0C0 &&
+ (int)RevoCalibration::MAG_TRANSFORM_R1C0 == (int)AuxMagSettings::MAG_TRANSFORM_R1C0 &&
+ (int)RevoCalibration::MAG_TRANSFORM_R2C0 == (int)AuxMagSettings::MAG_TRANSFORM_R2C0 &&
+ (int)RevoCalibration::MAG_TRANSFORM_R0C1 == (int)AuxMagSettings::MAG_TRANSFORM_R0C1 &&
+ (int)RevoCalibration::MAG_TRANSFORM_R1C1 == (int)AuxMagSettings::MAG_TRANSFORM_R1C1 &&
+ (int)RevoCalibration::MAG_TRANSFORM_R2C1 == (int)AuxMagSettings::MAG_TRANSFORM_R2C1 &&
+ (int)RevoCalibration::MAG_TRANSFORM_R0C2 == (int)AuxMagSettings::MAG_TRANSFORM_R0C2 &&
+ (int)RevoCalibration::MAG_TRANSFORM_R1C2 == (int)AuxMagSettings::MAG_TRANSFORM_R1C2 &&
+ (int)RevoCalibration::MAG_TRANSFORM_R2C2 == (int)AuxMagSettings::MAG_TRANSFORM_R2C2 &&
+ (int)RevoCalibration::MAG_BIAS_X == (int)AuxMagSettings::MAG_BIAS_X &&
+ (int)RevoCalibration::MAG_BIAS_Y == (int)AuxMagSettings::MAG_BIAS_Y &&
+ (int)RevoCalibration::MAG_BIAS_Z == (int)AuxMagSettings::MAG_BIAS_Z);
+
calibrationStepsAccelOnly.clear();
calibrationStepsAccelOnly << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED,
tr("Place horizontally and press Save Position..."))
@@ -85,14 +101,20 @@ SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
+ auxMagSettings = AuxMagSettings::GetInstance(getObjectManager());
+ Q_ASSERT(auxMagSettings);
+
accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
Q_ASSERT(accelGyroSettings);
accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
- magState = MagState::GetInstance(getObjectManager());
- Q_ASSERT(magState);
+ magSensor = MagSensor::GetInstance(getObjectManager());
+ Q_ASSERT(magSensor);
+
+ auxMagSensor = AuxMagSensor::GetInstance(getObjectManager());
+ Q_ASSERT(auxMagSensor);
homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
@@ -166,6 +188,26 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag)
revoCalibration->setData(revoCalibrationData);
+ // Calibration AuxMag
+ AuxMagSettings::DataFields auxMagSettingsData = auxMagSettings->getData();
+ memento.auxMagSettings = auxMagSettingsData;
+
+ // Reset the transformation matrix to identity
+ for (int i = 0; i < AuxMagSettings::MAG_TRANSFORM_R2C2; i++) {
+ auxMagSettingsData.mag_transform[i] = 0;
+ }
+ auxMagSettingsData.mag_transform[AuxMagSettings::MAG_TRANSFORM_R0C0] = 1;
+ auxMagSettingsData.mag_transform[AuxMagSettings::MAG_TRANSFORM_R1C1] = 1;
+ auxMagSettingsData.mag_transform[AuxMagSettings::MAG_TRANSFORM_R2C2] = 1;
+ auxMagSettingsData.mag_bias[AuxMagSettings::MAG_BIAS_X] = 0;
+ auxMagSettingsData.mag_bias[AuxMagSettings::MAG_BIAS_Y] = 0;
+ auxMagSettingsData.mag_bias[AuxMagSettings::MAG_BIAS_Z] = 0;
+
+ // Disable adaptive mag nulling
+ auxMagSettingsData.MagBiasNullingRate = 0;
+
+ auxMagSettings->setData(auxMagSettingsData);
+
QThread::usleep(100000);
mag_accum_x.clear();
@@ -186,12 +228,19 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag)
}
// Need to get as many mag updates as possible
- memento.magStateMetadata = magState->getMetadata();
+ memento.magSensorMetadata = magSensor->getMetadata();
+ memento.auxMagSensorMetadata = auxMagSensor->getMetadata();
+
if (calibrateMag) {
- UAVObject::Metadata mdata = magState->getMetadata();
+ UAVObject::Metadata mdata = magSensor->getMetadata();
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
- magState->setMetadata(mdata);
+ magSensor->setMetadata(mdata);
+
+ mdata = auxMagSensor->getMetadata();
+ UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
+ mdata.flightTelemetryUpdatePeriod = 100;
+ auxMagSensor->setMetadata(mdata);
}
// reset dirty state to forget previous unsaved runs
@@ -229,6 +278,9 @@ void SixPointCalibrationModel::savePositionData()
mag_accum_x.clear();
mag_accum_y.clear();
mag_accum_z.clear();
+ aux_mag_accum_x.clear();
+ aux_mag_accum_y.clear();
+ aux_mag_accum_z.clear();
collectingData = true;
@@ -236,10 +288,12 @@ void SixPointCalibrationModel::savePositionData()
#ifdef FITTING_USING_CONTINOUS_ACQUISITION
// Mag samples are acquired during the whole calibration session, to be used for ellipsoid fit.
if (!position) {
- connect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
+ connect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
+ connect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
}
#endif // FITTING_USING_CONTINOUS_ACQUISITION
- connect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
+ connect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
+ connect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
}
if (calibratingAccel) {
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
@@ -264,8 +318,8 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
accel_accum_x.append(accelStateData.x);
accel_accum_y.append(accelStateData.y);
accel_accum_z.append(accelStateData.z);
- } else if (obj->getObjID() == MagState::OBJID) {
- MagState::DataFields magData = magState->getData();
+ } else if (obj->getObjID() == MagSensor::OBJID) {
+ MagSensor::DataFields magData = magSensor->getData();
mag_accum_x.append(magData.x);
mag_accum_y.append(magData.y);
mag_accum_z.append(magData.z);
@@ -274,6 +328,19 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
mag_fit_y.append(magData.y);
mag_fit_z.append(magData.z);
#endif // FITTING_USING_CONTINOUS_ACQUISITION
+ } else if (obj->getObjID() == AuxMagSensor::OBJID) {
+ AuxMagSensor::DataFields auxMagData = auxMagSensor->getData();
+ if (auxMagData.Status == AuxMagSensor::STATUS_OK) {
+ aux_mag_accum_x.append(auxMagData.x);
+ aux_mag_accum_y.append(auxMagData.y);
+ aux_mag_accum_z.append(auxMagData.z);
+ calibratingAuxMag = true;
+#ifndef FITTING_USING_CONTINOUS_ACQUISITION
+ aux_mag_fit_x.append(auxMagData.x);
+ aux_mag_fit_y.append(auxMagData.y);
+ aux_mag_fit_z.append(auxMagData.z);
+#endif // FITTING_USING_CONTINOUS_ACQUISITION
+ }
} else {
Q_ASSERT(0);
}
@@ -307,10 +374,8 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
// Store the mean for this position for the mag
if (calibratingMag) {
- disconnect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
- mag_data_x[position] = CalibrationUtils::listMean(mag_accum_x);
- mag_data_y[position] = CalibrationUtils::listMean(mag_accum_y);
- mag_data_z[position] = CalibrationUtils::listMean(mag_accum_z);
+ disconnect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
+ disconnect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
}
position = (position + 1) % 6;
@@ -322,17 +387,19 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
// done...
#ifdef FITTING_USING_CONTINOUS_ACQUISITION
if (calibratingMag) {
- disconnect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
+ disconnect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
+ disconnect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
}
#endif // FITTING_USING_CONTINOUS_ACQUISITION
compute();
// Restore original settings
accelState->setMetadata(memento.accelStateMetadata);
- magState->setMetadata(memento.magStateMetadata);
+ magSensor->setMetadata(memento.magSensorMetadata);
+ auxMagSensor->setMetadata(memento.auxMagSensorMetadata);
revoCalibration->setData(memento.revoCalibrationData);
accelGyroSettings->setData(memento.accelGyroSettingsData);
-
+ auxMagSettings->setData(memento.auxMagSettings);
// Recall saved board rotation
recallBoardRotation();
@@ -347,11 +414,19 @@ void SixPointCalibrationModel::continouslyGetMagSamples(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
- if (obj->getObjID() == MagState::OBJID) {
- MagState::DataFields magStateData = magState->getData();
- mag_fit_x.append(magStateData.x);
- mag_fit_y.append(magStateData.y);
- mag_fit_z.append(magStateData.z);
+ if (obj->getObjID() == MagSensor::OBJID) {
+ MagSensor::DataFields magSensorData = magSensor->getData();
+ mag_fit_x.append(magSensorData.x);
+ mag_fit_y.append(magSensorData.y);
+ mag_fit_z.append(magSensorData.z);
+ } else if (obj->getObjID() == AuxMagSensor::OBJID) {
+ AuxMagSensor::DataFields auxMagData = auxMagSensor->getData();
+ if (auxMagData.Status == AuxMagSensor::STATUS_OK) {
+ aux_mag_fit_x.append(auxMagData.x);
+ aux_mag_fit_y.append(auxMagData.y);
+ aux_mag_fit_z.append(auxMagData.z);
+ calibratingAuxMag = true;
+ }
}
}
@@ -365,6 +440,7 @@ void SixPointCalibrationModel::compute()
double Be_length;
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
+ AuxMagSettings::DataFields auxCalibrationData = auxMagSettings->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
HomeLocation::DataFields homeLocationData = homeLocation->getData();
@@ -384,51 +460,21 @@ void SixPointCalibrationModel::compute()
// Calibration mag
if (calibratingMag) {
Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2));
- int vectSize = mag_fit_x.count();
- Eigen::VectorXf samples_x(vectSize);
- Eigen::VectorXf samples_y(vectSize);
- Eigen::VectorXf samples_z(vectSize);
- for (int i = 0; i < vectSize; i++) {
- samples_x(i) = mag_fit_x[i];
- samples_y(i) = mag_fit_y[i];
- samples_z(i) = mag_fit_z[i];
+
+ qDebug() << "-----------------------------------";
+ qDebug() << "Onboard Mag";
+ calcCalibration(mag_fit_x, mag_fit_y, mag_fit_z, Be_length, revoCalibrationData.mag_transform, revoCalibrationData.mag_bias);
+ if (calibratingAuxMag) {
+ qDebug() << "Aux Mag";
+ calcCalibration(aux_mag_fit_x, aux_mag_fit_y, aux_mag_fit_z, Be_length, auxCalibrationData.mag_transform, auxCalibrationData.mag_bias);
}
- OpenPilot::CalibrationUtils::EllipsoidCalibrationResult result;
- OpenPilot::CalibrationUtils::EllipsoidCalibration(&samples_x, &samples_y, &samples_z, Be_length, &result, true);
- qDebug() << "-----------------------------------";
- qDebug() << "Mag Calibration results: Fit";
- qDebug() << "scale(" << result.Scale.coeff(0) << ", " << result.Scale.coeff(1) << ", " << result.Scale.coeff(2) << ")";
- qDebug() << "bias(" << result.Bias.coeff(0) << ", " << result.Bias.coeff(1) << ", " << result.Bias.coeff(2) << ")";
-
- OpenPilot::CalibrationUtils::SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
-
- qDebug() << "-----------------------------------";
- qDebug() << "Mag Calibration results: Six Point";
- qDebug() << "scale(" << S[0] << ", " << S[1] << ", " << S[2] << ")";
- qDebug() << "bias(" << -sign(S[0]) * b[0] << ", " << -sign(S[1]) * b[1] << ", " << -sign(S[2]) * b[2] << ")";
- qDebug() << "-----------------------------------";
-
- revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = result.CalibrationMatrix.coeff(0, 0);
- revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1] = result.CalibrationMatrix.coeff(0, 1);
- revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2] = result.CalibrationMatrix.coeff(0, 2);
-
- revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0] = result.CalibrationMatrix.coeff(1, 0);
- revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = result.CalibrationMatrix.coeff(1, 1);
- revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2] = result.CalibrationMatrix.coeff(1, 2);
-
- revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0] = result.CalibrationMatrix.coeff(2, 0);
- revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1] = result.CalibrationMatrix.coeff(2, 1);
- revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = result.CalibrationMatrix.coeff(2, 2);
-
- revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = result.Bias.coeff(0);
- revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = result.Bias.coeff(1);
- revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = result.Bias.coeff(2);
}
// Restore the previous setting
revoCalibrationData.MagBiasNullingRate = memento.revoCalibrationData.MagBiasNullingRate;;
// Check the mag calibration is good
- bool good_calibration = true;
+ bool good_calibration = true;
+ bool good_aux_calibration = true;
if (calibratingMag) {
good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0]);
good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1]);
@@ -445,6 +491,23 @@ void SixPointCalibrationModel::compute()
good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X]);
good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]);
good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]);
+ if (calibratingAuxMag) {
+ good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0]);
+ good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1]);
+ good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2]);
+
+ good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0]);
+ good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1]);
+ good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2]);
+
+ good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0]);
+ good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1]);
+ good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2]);
+
+ good_calibration &= !IS_NAN(auxCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X]);
+ good_calibration &= !IS_NAN(auxCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]);
+ good_calibration &= !IS_NAN(auxCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]);
+ }
}
// Check the accel calibration is good
if (calibratingAccel) {
@@ -458,7 +521,8 @@ void SixPointCalibrationModel::compute()
if (good_calibration) {
m_dirty = true;
if (calibratingMag) {
- result.revoCalibrationData = revoCalibrationData;
+ result.revoCalibrationData = revoCalibrationData;
+ result.auxMagSettingsData = auxCalibrationData;
displayInstructions(tr("Magnetometer calibration completed successfully."), WizardModel::Success);
}
if (calibratingAccel) {
@@ -473,6 +537,42 @@ void SixPointCalibrationModel::compute()
position = -1;
}
+void SixPointCalibrationModel::calcCalibration(QList<float> x, QList<float> y, QList<float> z, double Be_length, float calibrationMatrix[], float bias[])
+{
+ int vectSize = x.count();
+ Eigen::VectorXf samples_x(vectSize);
+ Eigen::VectorXf samples_y(vectSize);
+ Eigen::VectorXf samples_z(vectSize);
+
+ for (int i = 0; i < vectSize; i++) {
+ samples_x(i) = x[i];
+ samples_y(i) = y[i];
+ samples_z(i) = z[i];
+ }
+ OpenPilot::CalibrationUtils::EllipsoidCalibrationResult result;
+ OpenPilot::CalibrationUtils::EllipsoidCalibration(&samples_x, &samples_y, &samples_z, Be_length, &result, true);
+
+ qDebug() << "Mag fitting results: ";
+ qDebug() << "scale(" << result.Scale.coeff(0) << ", " << result.Scale.coeff(1) << ", " << result.Scale.coeff(2) << ")";
+ qDebug() << "bias(" << result.Bias.coeff(0) << ", " << result.Bias.coeff(1) << ", " << result.Bias.coeff(2) << ")";
+ qDebug() << "-----------------------------------";
+ calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R0C0] = result.CalibrationMatrix.coeff(0, 0);
+ calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R0C1] = result.CalibrationMatrix.coeff(0, 1);
+ calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R0C2] = result.CalibrationMatrix.coeff(0, 2);
+
+ calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R1C0] = result.CalibrationMatrix.coeff(1, 0);
+ calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R1C1] = result.CalibrationMatrix.coeff(1, 1);
+ calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R1C2] = result.CalibrationMatrix.coeff(1, 2);
+
+ calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R2C0] = result.CalibrationMatrix.coeff(2, 0);
+ calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R2C1] = result.CalibrationMatrix.coeff(2, 1);
+ calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R2C2] = result.CalibrationMatrix.coeff(2, 2);
+
+ bias[RevoCalibration::MAG_BIAS_X] = result.Bias.coeff(0);
+ bias[RevoCalibration::MAG_BIAS_Y] = result.Bias.coeff(1);
+ bias[RevoCalibration::MAG_BIAS_Z] = result.Bias.coeff(2);
+}
+
void SixPointCalibrationModel::save()
{
if (!m_dirty) {
@@ -489,6 +589,18 @@ void SixPointCalibrationModel::save()
}
revoCalibration->setData(revoCalibrationData);
+ if (calibratingAuxMag) {
+ AuxMagSettings::DataFields auxCalibrationData = auxMagSettings->getData();
+ // Note that Revo/AuxMag MAG_TRANSFORM_RxCx are interchangeable, an assertion at initialization enforces the structs are equal
+ for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_NUMELEM; i++) {
+ auxCalibrationData.mag_transform[i] = result.auxMagSettingsData.mag_transform[i];
+ }
+ for (int i = 0; i < 3; i++) {
+ auxCalibrationData.mag_bias[i] = result.auxMagSettingsData.mag_bias[i];
+ }
+
+ auxMagSettings->setData(auxCalibrationData);
+ }
}
if (calibratingAccel) {
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h
index 37c7b7d29..b398aeb0c 100644
--- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h
+++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h
@@ -31,10 +31,13 @@
#include "wizardmodel.h"
#include "calibration/calibrationutils.h"
#include <revocalibration.h>
+
+#include <auxmagsettings.h>
#include <accelgyrosettings.h>
#include <homelocation.h>
#include <accelstate.h>
-#include <magstate.h>
+#include <magsensor.h>
+#include <auxmagsensor.h>
#include <QMutex>
#include <QObject>
@@ -87,17 +90,21 @@ public:
typedef struct {
UAVObject::Metadata accelStateMetadata;
- UAVObject::Metadata magStateMetadata;
- RevoCalibration::DataFields revoCalibrationData;
+ UAVObject::Metadata magSensorMetadata;
+ UAVObject::Metadata auxMagSensorMetadata;
+ AuxMagSettings::DataFields auxMagSettings;
+ RevoCalibration::DataFields revoCalibrationData;
AccelGyroSettings::DataFields accelGyroSettingsData;
} Memento;
typedef struct {
RevoCalibration::DataFields revoCalibrationData;
+ AuxMagSettings::DataFields auxMagSettingsData;
AccelGyroSettings::DataFields accelGyroSettingsData;
} Result;
bool calibratingMag;
+ bool calibratingAuxMag;
bool calibratingAccel;
QList<CalibrationStep> calibrationStepsMag;
@@ -114,11 +121,11 @@ public:
QMutex sensorsUpdateLock;
double accel_data_x[6], accel_data_y[6], accel_data_z[6];
- double mag_data_x[6], mag_data_y[6], mag_data_z[6];
QList<double> accel_accum_x;
QList<double> accel_accum_y;
QList<double> accel_accum_z;
+
QList<double> mag_accum_x;
QList<double> mag_accum_y;
QList<double> mag_accum_z;
@@ -126,11 +133,20 @@ public:
QList<float> mag_fit_y;
QList<float> mag_fit_z;
+ QList<double> aux_mag_accum_x;
+ QList<double> aux_mag_accum_y;
+ QList<double> aux_mag_accum_z;
+ QList<float> aux_mag_fit_x;
+ QList<float> aux_mag_fit_y;
+ QList<float> aux_mag_fit_z;
+
// convenience pointers
RevoCalibration *revoCalibration;
AccelGyroSettings *accelGyroSettings;
+ AuxMagSettings *auxMagSettings;
AccelState *accelState;
- MagState *magState;
+ MagSensor *magSensor;
+ AuxMagSensor *auxMagSensor;
HomeLocation *homeLocation;
void start(bool calibrateAccel, bool calibrateMag);
@@ -138,6 +154,7 @@ public:
void compute();
void showHelp(QString image);
UAVObjectManager *getObjectManager();
+ void calcCalibration(QList<float> x, QList<float> y, QList<float> z, double Be_length, float calibrationMatrix[], float bias[]);
};
}
diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
index 0f08a3536..ecf662715 100644
--- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
+++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
@@ -127,6 +127,9 @@ HEADERS += \
$$UAVOBJECT_SYNTHETICS/waypointactive.h \
$$UAVOBJECT_SYNTHETICS/mpu6000settings.h \
$$UAVOBJECT_SYNTHETICS/takeofflocation.h \
+ $$UAVOBJECT_SYNTHETICS/auxmagsensor.h \
+ $$UAVOBJECT_SYNTHETICS/auxmagsettings.h \
+ $$UAVOBJECT_SYNTHETICS/gpsextendedstatus.h \
$$UAVOBJECT_SYNTHETICS/perfcounter.h
SOURCES += \
@@ -231,5 +234,8 @@ SOURCES += \
$$UAVOBJECT_SYNTHETICS/waypointactive.cpp \
$$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \
$$UAVOBJECT_SYNTHETICS/takeofflocation.cpp \
+ $$UAVOBJECT_SYNTHETICS/auxmagsensor.cpp \
+ $$UAVOBJECT_SYNTHETICS/auxmagsettings.cpp \
+ $$UAVOBJECT_SYNTHETICS/gpsextendedstatus.cpp \
$$UAVOBJECT_SYNTHETICS/perfcounter.cpp
-
+
diff --git a/shared/uavobjectdefinition/auxmagsensor.xml b/shared/uavobjectdefinition/auxmagsensor.xml
new file mode 100644
index 000000000..1deb44da8
--- /dev/null
+++ b/shared/uavobjectdefinition/auxmagsensor.xml
@@ -0,0 +1,13 @@
+<xml>
+ <object name="AuxMagSensor" singleinstance="true" settings="false" category="Sensors">
+ <description>Calibrated sensor data from aux 3 axis magnetometer in MilliGauss.</description>
+ <field name="x" units="mGa" type="float" elements="1"/>
+ <field name="y" units="mGa" type="float" elements="1"/>
+ <field name="z" units="mGa" type="float" elements="1"/>
+ <field name="Status" units="" type="enum" elements="1" options="None,Ok" defaultvalue="None"/>
+ <access gcs="readwrite" flight="readwrite"/>
+ <telemetrygcs acked="false" updatemode="manual" period="0"/>
+ <telemetryflight acked="false" updatemode="periodic" period="10000"/>
+ <logging updatemode="manual" period="0"/>
+ </object>
+</xml>
diff --git a/shared/uavobjectdefinition/auxmagsettings.xml b/shared/uavobjectdefinition/auxmagsettings.xml
new file mode 100644
index 000000000..fda802031
--- /dev/null
+++ b/shared/uavobjectdefinition/auxmagsettings.xml
@@ -0,0 +1,16 @@
+<xml>
+ <object name="AuxMagSettings" singleinstance="true" settings="true" category="Sensors">
+ <description>Settings for auxiliary magnetometer calibration</description>
+ <field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
+ <field name="mag_transform" units="gain" type="float" elementnames="r0c0,r0c1,r0c2,r1c0,r1c1,r1c2,r2c0,r2c1,r2c2"
+ defaultvalue="1,0,0,0,1,0,0,0,1"/>
+ <field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/>
+ <field name="Orientation" units="degrees" type="float" elements="1" defaultvalue="0"/>
+ <field name="Type" units="" type="enum" elements="1" options="GPSV9,Ext" defaultvalue="GPSV9"/>
+ <field name="Usage" units="" type="enum" elements="1" options="Both,OnboardOnly,AuxOnly" defaultvalue="Both"/>
+ <access gcs="readwrite" flight="readwrite"/>
+ <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+ <telemetryflight acked="true" updatemode="onchange" period="0"/>
+ <logging updatemode="manual" period="0"/>
+ </object>
+</xml>
diff --git a/shared/uavobjectdefinition/gpsextendedstatus.xml b/shared/uavobjectdefinition/gpsextendedstatus.xml
new file mode 100644
index 000000000..d5da25352
--- /dev/null
+++ b/shared/uavobjectdefinition/gpsextendedstatus.xml
@@ -0,0 +1,15 @@
+<xml>
+ <object name="GPSExtendedStatus" singleinstance="true" settings="false" category="Sensors">
+ <description>Extended GPS status.</description>
+ <field name="Status" units="" type="enum" elements="1" options="NONE,GPSV9" defaultvalue="NONE"/>
+ <field name="FlightTime" units="" type="uint32" elements="1"/>
+ <field name="HeapRemaining" units="bytes" type="uint32" elements="1"/>
+ <field name="IRQStackRemaining" units="bytes" type="uint16" elements="1"/>
+ <field name="SysModStackRemaining" units="bytes" type="uint16" elements="1"/>
+ <field name="Options" units="" type="uint16" elements="1"/>
+ <access gcs="readwrite" flight="readwrite"/>
+ <telemetrygcs acked="false" updatemode="manual" period="0"/>
+ <telemetryflight acked="false" updatemode="periodic" period="1000"/>
+ <logging updatemode="manual" period="0"/>
+ </object>
+</xml>
diff --git a/shared/uavobjectdefinition/gpspositionsensor.xml b/shared/uavobjectdefinition/gpspositionsensor.xml
index cfaba9db0..a6a5ea194 100644
--- a/shared/uavobjectdefinition/gpspositionsensor.xml
+++ b/shared/uavobjectdefinition/gpspositionsensor.xml
@@ -12,6 +12,8 @@
<field name="PDOP" units="" type="float" elements="1"/>
<field name="HDOP" units="" type="float" elements="1"/>
<field name="VDOP" units="" type="float" elements="1"/>
+ <field name="SensorType" units="" type="enum" elements="1" options="Unknown,NMEA,UBX,UBX7,UBX8" defaultvalue="Unknown" />
+ <field name="AutoConfigStatus" units="" type="enum" elements="1" options="DISABLED,RUNNING,DONE,ERROR" defaultvalue="DISABLED" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
diff --git a/shared/uavobjectdefinition/gpssettings.xml b/shared/uavobjectdefinition/gpssettings.xml
index dd4dd8cd5..e2919eec8 100644
--- a/shared/uavobjectdefinition/gpssettings.xml
+++ b/shared/uavobjectdefinition/gpssettings.xml
@@ -2,8 +2,20 @@
<object name="GPSSettings" singleinstance="true" settings="true" category="Sensors">
<description>GPS Module specific settings</description>
<field name="DataProtocol" units="" type="enum" elements="1" options="NMEA,UBX" defaultvalue="UBX"/>
- <field name="MinSattelites" units="" type="uint8" elements="1" defaultvalue="7"/>
+ <field name="MinSatellites" units="" type="uint8" elements="1" defaultvalue="7"/>
<field name="MaxPDOP" units="" type="float" elements="1" defaultvalue="3.5"/>
+ <!-- Ubx self configuration. Enable to allow the flight board to auto configure ubx GPS options,
+ store does AutoConfig and save GPS settings (i.e. to prevent issues if gps is power cycled) -->
+ <field name="UbxAutoConfig" units="" type="enum" elements="1" options="Disabled,Configure,ConfigureAndStore" defaultvalue="Disabled"/>
+ <!-- Ubx position update rate, -1 for auto -->
+ <field name="UbxRate" units="Hz" type="int8" elements="1" defaultvalue="5" />
+ <!-- Ubx dynamic model, see UBX datasheet for more details -->
+ <field name="UbxDynamicModel" units="" type="enum" elements="1"
+ options="Portable,Stationary,Pedestrian,Automotive,Sea,Airborne1G,Airborne2G,Airborne4G" defaultvalue="Airborne1G" />
+ <!-- Ubx SBAS settings -->
+ <field name="UbxSBASMode" units="" type="enum" elements="1" options="Disabled,Ranging,Correction,Integrity,Ranging+Correction,Ranging+Integrity,Ranging+Correction+Integrity,Correction+Integrity" defaultvalue="Ranging" />
+ <field name="UbxSBASChannelsUsed" units="" type="uint8" elements="1" defaultvalue="3"/>
+ <field name="UbxSBASSats" units="" type="enum" elements="1" options="AutoScan,WAAS,EGNOS,MSAS,GAGAN,SDCM" defaultvalue="Auto-Scan" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml
index 7fa821da4..0f24744a4 100644
--- a/shared/uavobjectdefinition/hwsettings.xml
+++ b/shared/uavobjectdefinition/hwsettings.xml
@@ -17,7 +17,7 @@
<field name="RM_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge,OsdHk" defaultvalue="Disabled"/>
<field name="TelemetrySpeed" units="bps" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200" defaultvalue="57600"/>
- <field name="GPSSpeed" units="bps" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200" defaultvalue="57600"/>
+ <field name="GPSSpeed" units="bps" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200,230400" defaultvalue="57600"/>
<field name="ComUsbBridgeSpeed" units="bps" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200" defaultvalue="57600"/>
<field name="USB_HIDPort" units="function" type="enum" elements="1" options="USBTelemetry,RCTransmitter,Disabled" defaultvalue="USBTelemetry"/>
<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,DebugConsole,Disabled" defaultvalue="Disabled"/>
diff --git a/shared/uavobjectdefinition/magstate.xml b/shared/uavobjectdefinition/magstate.xml
index 1cd0fefaa..0de27ce17 100644
--- a/shared/uavobjectdefinition/magstate.xml
+++ b/shared/uavobjectdefinition/magstate.xml
@@ -1,9 +1,10 @@
<xml>
<object name="MagState" singleinstance="true" settings="false" category="State">
<description>The filtered magnet vector.</description>
- <field name="x" units="mGa" type="float" elements="1"/>
- <field name="y" units="mGa" type="float" elements="1"/>
- <field name="z" units="mGa" type="float" elements="1"/>
+ <field name="x" units="mGa" type="float" elements="1"/>
+ <field name="y" units="mGa" type="float" elements="1"/>
+ <field name="z" units="mGa" type="float" elements="1"/>
+ <field name="Source" units="" type="enum" elements="1" options="Invalid,OnBoard,Aux" defaultvalue="Invalid"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>