rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merge branch 'next' into hayvosh/OP-1489_ILimits

Browse Source
This commit is contained in:
Fredrik Larson 2014-09-17 02:25:53 +10:00
commit 20b635fa07
35 changed files with 1663 additions and 207 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CREDITS.txt
View File

@ -46,6 +46,7 @@ Patrick Huebner
Ryan Hunt
Mark James
Michael Johnston
Stefan Karlsson
Ricky King
Thorsten Klose
Karl Knutsson

72
flight/libraries/auxmagsupport.c Normal file
View File

@ -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;
}

68
flight/libraries/inc/CoordinateConversions.h
View File

@ -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_

51
flight/libraries/inc/auxmagsupport.h Normal file
View File

@ -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_ */

145
flight/modules/GPS/GPS.c
View File

@ -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 */
/**
* @}
* @}

2
flight/modules/GPS/NMEA.c
View File

@ -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;
}

223
flight/modules/GPS/UBX.c
View File

@ -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

1
flight/modules/GPS/inc/GPS.h
View File

@ -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

149
flight/modules/GPS/inc/UBX.h
View File

@ -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 */

185
flight/modules/GPS/inc/ubx_autoconfig.h Normal file
View File

@ -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_ */

399
flight/modules/GPS/ubx_autoconfig.c Normal file
View File

@ -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;
}

2
flight/modules/StateEstimation/filterlla.c
View File

@ -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] = {

138
flight/modules/StateEstimation/filtermag.c
View File

@ -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
}
/**
* @}
* @}

24
flight/modules/StateEstimation/inc/stateestimation.h
View File

@ -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;

46
flight/modules/StateEstimation/stateestimation.c
View File

@ -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()) {

2
flight/targets/boards/coptercontrol/firmware/Makefile
View File

@ -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

1
flight/targets/boards/discoveryf4bare/firmware/Makefile
View File

@ -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

3
flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
View File

@ -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

3
flight/targets/boards/osd/firmware/Makefile
View File

@ -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

1
flight/targets/boards/revolution/firmware/Makefile
View File

@ -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

3
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -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

5
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -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:

1
flight/targets/boards/revoproto/firmware/Makefile
View File

@ -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

3
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -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

2
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -36,6 +36,8 @@ UAVOBJSRCFILENAMES += gyrosensor
UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += auxmagsensor
UAVOBJSRCFILENAMES += auxmagsettings
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor

236
ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp
View File

@ -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();

27
ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h
View File

@ -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[]);
};
}

8
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -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

13
shared/uavobjectdefinition/auxmagsensor.xml Normal file
View File

@ -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>

16
shared/uavobjectdefinition/auxmagsettings.xml Normal file
View File

@ -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>

15
shared/uavobjectdefinition/gpsextendedstatus.xml Normal file
View File

@ -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>

2
shared/uavobjectdefinition/gpspositionsensor.xml
View File

@ -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"/>

14
shared/uavobjectdefinition/gpssettings.xml
View File

@ -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"/>

2
shared/uavobjectdefinition/hwsettings.xml
View File

@ -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"/>

7
shared/uavobjectdefinition/magstate.xml
View File

@ -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"/>