From dfb21482f467f426d46d5b719f8ae89d24579548 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gast=C3=A9=20Olivier?= Date: Fri, 8 Aug 2014 13:14:39 +0200 Subject: [PATCH 01/74] DSM protocol improvement added item in DSM protocol in GCS --- flight/pios/inc/pios_dsm_priv.h | 3 +- flight/pios/stm32f4xx/pios_dsm.c | 41 +++++-------------- .../coptercontrol/firmware/pios_board.c | 10 +++-- .../boards/revolution/firmware/pios_board.c | 20 ++++++--- .../plugins/setupwizard/connectiondiagram.cpp | 3 +- .../plugins/setupwizard/pages/inputpage.cpp | 18 +++++--- .../src/plugins/setupwizard/setupwizard.cpp | 11 +++-- .../vehicleconfigurationhelper.cpp | 17 +++++--- .../setupwizard/vehicleconfigurationsource.h | 2 +- shared/uavobjectdefinition/hwsettings.xml | 14 +++---- 10 files changed, 76 insertions(+), 63 deletions(-) diff --git a/flight/pios/inc/pios_dsm_priv.h b/flight/pios/inc/pios_dsm_priv.h index 1cdf88f75..d823b4f3d 100644 --- a/flight/pios/inc/pios_dsm_priv.h +++ b/flight/pios/inc/pios_dsm_priv.h @@ -111,7 +111,8 @@ /* DSM protocol variations */ enum pios_dsm_proto { - PIOS_DSM_PROTO_DSM2, + PIOS_DSM_PROTO_DSM210BIT, + PIOS_DSM_PROTO_DSM211BIT, PIOS_DSM_PROTO_DSMX10BIT, PIOS_DSM_PROTO_DSMX11BIT, }; diff --git a/flight/pios/stm32f4xx/pios_dsm.c b/flight/pios/stm32f4xx/pios_dsm.c index b6d8f0d85..70b32483d 100644 --- a/flight/pios/stm32f4xx/pios_dsm.c +++ b/flight/pios/stm32f4xx/pios_dsm.c @@ -181,7 +181,7 @@ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) { struct pios_dsm_state *state = &(dsm_dev->state); - uint8_t resolution; + uint8_t resolution=10; #ifdef DSM_LOST_FRAME_COUNTER /* increment the lost frame counter */ @@ -191,35 +191,16 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) #endif /* check the frame type assuming master satellite stream */ - uint8_t type = state->received_data[1]; - switch (type) { - case 0x01: - case 0x02: - case 0x12: - /* DSM2, DSMJ stream */ - if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) { - /* DSM2/DSMJ resolution is known from the header */ - resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10; - } else { - /* DSMX resolution should explicitly be selected */ - goto stream_error; - } - break; - case 0xA2: - case 0xB2: - /* DSMX stream */ - if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) { - resolution = 10; - } else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) { - resolution = 11; - } else { - /* DSMX resolution should explicitly be selected */ - goto stream_error; - } - break; - default: - /* unknown yet data stream */ - goto stream_error; + + switch(dsm_dev->proto) { + case PIOS_DSM_PROTO_DSM210BIT: + case PIOS_DSM_PROTO_DSMX10BIT: + resolution = 10; + break; + case PIOS_DSM_PROTO_DSM211BIT: + case PIOS_DSM_PROTO_DSMX11BIT: + resolution = 11; + break; } /* unroll channels */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index 2cb1734ff..35029acd6 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -477,15 +477,19 @@ void PIOS_Board_Init(void) } #endif /* PIOS_INCLUDE_GPS */ break; - case HWSETTINGS_CC_MAINPORT_DSM2: + case HWSETTINGS_CC_MAINPORT_DSM210BIT: + case HWSETTINGS_CC_MAINPORT_DSM211BIT: case HWSETTINGS_CC_MAINPORT_DSMX10BIT: case HWSETTINGS_CC_MAINPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) { enum pios_dsm_proto proto; switch (hwsettings_cc_mainport) { - case HWSETTINGS_CC_MAINPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; + case HWSETTINGS_CC_MAINPORT_DSM210BIT: + proto = PIOS_DSM_PROTO_DSM210BIT; + break; + case HWSETTINGS_CC_MAINPORT_DSM211BIT: + proto = PIOS_DSM_PROTO_DSM211BIT; break; case HWSETTINGS_CC_MAINPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 4901cc432..ebbedfb19 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -624,14 +624,18 @@ void PIOS_Board_Init(void) } #endif break; - case HWSETTINGS_RM_MAINPORT_DSM2: + case HWSETTINGS_RM_MAINPORT_DSM210BIT: + case HWSETTINGS_RM_MAINPORT_DSM211BIT: case HWSETTINGS_RM_MAINPORT_DSMX10BIT: case HWSETTINGS_RM_MAINPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hwsettings_mainport) { - case HWSETTINGS_RM_MAINPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; + case HWSETTINGS_RM_MAINPORT_DSM210BIT: + proto = PIOS_DSM_PROTO_DSM210BIT; + break; + case HWSETTINGS_RM_MAINPORT_DSM211BIT: + proto = PIOS_DSM_PROTO_DSM211BIT; break; case HWSETTINGS_RM_MAINPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; @@ -693,14 +697,18 @@ void PIOS_Board_Init(void) 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); break; - case HWSETTINGS_RM_FLEXIPORT_DSM2: + case HWSETTINGS_RM_FLEXIPORT_DSM210BIT: + case HWSETTINGS_RM_FLEXIPORT_DSM211BIT: case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hwsettings_flexiport) { - case HWSETTINGS_RM_FLEXIPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; + case HWSETTINGS_RM_FLEXIPORT_DSM210BIT: + proto = PIOS_DSM_PROTO_DSM210BIT; + break; + case HWSETTINGS_RM_FLEXIPORT_DSM211BIT: + proto = PIOS_DSM_PROTO_DSM211BIT; break; case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp index 22b85bdd3..8b1f56d92 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp @@ -139,7 +139,8 @@ void ConnectionDiagram::setupGraphicsScene() break; case VehicleConfigurationSource::INPUT_DSMX10: case VehicleConfigurationSource::INPUT_DSMX11: - case VehicleConfigurationSource::INPUT_DSM2: + case VehicleConfigurationSource::INPUT_DSM2_10: + case VehicleConfigurationSource::INPUT_DSM2_11: elementsToShow << "satellite"; break; default: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp index 5515a31dd..5c9ec16b7 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp @@ -54,7 +54,7 @@ bool InputPage::validatePage() } else if (ui->sbusButton->isChecked()) { getWizard()->setInputType(SetupWizard::INPUT_SBUS); } else if (ui->spectrumButton->isChecked()) { - getWizard()->setInputType(SetupWizard::INPUT_DSM2); + getWizard()->setInputType(SetupWizard::INPUT_DSM2_11); } else { getWizard()->setInputType(SetupWizard::INPUT_PWM); } @@ -85,9 +85,13 @@ bool InputPage::restartNeeded(VehicleConfigurationSource::INPUT_TYPE selectedTyp case VehicleConfigurationSource::INPUT_SBUS: return data.CC_MainPort != HwSettings::CC_MAINPORT_SBUS; - case VehicleConfigurationSource::INPUT_DSM2: + case VehicleConfigurationSource::INPUT_DSM2_10: // TODO: Handle all of the DSM types ?? Which is most common? - return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM2; + return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM210BIT; + + case VehicleConfigurationSource::INPUT_DSM2_11: + // TODO: Handle all of the DSM types ?? Which is most common? + return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM211BIT; default: return true; } @@ -105,9 +109,13 @@ bool InputPage::restartNeeded(VehicleConfigurationSource::INPUT_TYPE selectedTyp case VehicleConfigurationSource::INPUT_SBUS: return data.RM_MainPort != HwSettings::CC_MAINPORT_SBUS; - case VehicleConfigurationSource::INPUT_DSM2: + case VehicleConfigurationSource::INPUT_DSM2_10: // TODO: Handle all of the DSM types ?? Which is most common? - return data.RM_MainPort != HwSettings::CC_MAINPORT_DSM2; + return data.RM_MainPort != HwSettings::CC_MAINPORT_DSM210BIT; + + case VehicleConfigurationSource::INPUT_DSM2_11: + // TODO: Handle all of the DSM types ?? Which is most common? + return data.RM_MainPort != HwSettings::CC_MAINPORT_DSM211BIT; default: return true; } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index 1657ca526..cc2cfd253 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -252,14 +252,17 @@ QString SetupWizard::getSummaryText() case INPUT_SBUS: summary.append(tr("Futaba S.Bus")); break; - case INPUT_DSM2: - summary.append(tr("Spektrum satellite (DSM2)")); + case INPUT_DSM2_10: + summary.append(tr("Spektrum satellite (DSM2 10bits)")); + break; + case INPUT_DSM2_11: + summary.append(tr("Spektrum satellite (DSM2 11bits)")); break; case INPUT_DSMX10: - summary.append(tr("Spektrum satellite (DSMX10BIT)")); + summary.append(tr("Spektrum satellite (DSMX 10bits)")); break; case INPUT_DSMX11: - summary.append(tr("Spektrum satellite (DSMX11BIT)")); + summary.append(tr("Spektrum satellite (DSMX 11bits)")); break; default: summary.append(tr("Unknown")); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 119d58120..a318d2fd4 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -139,8 +139,11 @@ void VehicleConfigurationHelper::applyHardwareConfiguration() case VehicleConfigurationSource::INPUT_DSMX11: data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSMX11BIT; break; - case VehicleConfigurationSource::INPUT_DSM2: - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM2; + case VehicleConfigurationSource::INPUT_DSM2_10: + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM210BIT; + break; + case VehicleConfigurationSource::INPUT_DSM2_11: + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM211BIT; break; default: break; @@ -172,8 +175,11 @@ void VehicleConfigurationHelper::applyHardwareConfiguration() case VehicleConfigurationSource::INPUT_DSMX11: data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSMX11BIT; break; - case VehicleConfigurationSource::INPUT_DSM2: - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM2; + case VehicleConfigurationSource::INPUT_DSM2_10: + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM210BIT; + break; + case VehicleConfigurationSource::INPUT_DSM2_11: + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM211BIT; break; default: break; @@ -486,7 +492,8 @@ void VehicleConfigurationHelper::applyManualControlDefaults() break; case VehicleConfigurationSource::INPUT_DSMX10: case VehicleConfigurationSource::INPUT_DSMX11: - case VehicleConfigurationSource::INPUT_DSM2: + case VehicleConfigurationSource::INPUT_DSM2_10: + case VehicleConfigurationSource::INPUT_DSM2_11: channelType = ManualControlSettings::CHANNELGROUPS_DSMMAINPORT; break; default: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index 797283656..3c0399421 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -62,7 +62,7 @@ public: MULTI_ROTOR_OCTO_X, MULTI_ROTOR_OCTO_V, MULTI_ROTOR_OCTO_COAX_X, MULTI_ROTOR_OCTO_COAX_PLUS, FIXED_WING_AILERON, FIXED_WING_VTAIL, HELI_CCPM }; enum ESC_TYPE { ESC_RAPID, ESC_LEGACY, ESC_UNKNOWN }; - enum INPUT_TYPE { INPUT_PWM, INPUT_PPM, INPUT_SBUS, INPUT_DSMX10, INPUT_DSMX11, INPUT_DSM2, INPUT_UNKNOWN }; + enum INPUT_TYPE { INPUT_PWM, INPUT_PPM, INPUT_SBUS, INPUT_DSMX10, INPUT_DSMX11, INPUT_DSM2_10, INPUT_DSM2_11, INPUT_UNKNOWN }; enum GPS_SETTING { GPS_UBX, GPS_NMEA, GPS_DISABLED }; enum RADIO_SETTING { RADIO_TELEMETRY, RADIO_DISABLED }; diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index f005a31ab..556397340 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -2,19 +2,19 @@ Selection of optional hardware configurations. - - + + - - - + + + - - + + From 52ad63be3364864fcd75c7701f6eedaf166d7f15 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 17 Jul 2014 20:57:27 +0200 Subject: [PATCH 02/74] OP-1406 - Add support for Aux magnetometer on flight controllers --- .../boards/revolution/firmware/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 ++ shared/uavobjectdefinition/auxmagsensor.xml | 13 +++++++++++++ 3 files changed, 16 insertions(+) create mode 100644 shared/uavobjectdefinition/auxmagsensor.xml diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 49867c39e..c0196527d 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -31,6 +31,7 @@ UAVOBJSRCFILENAMES += gyrosensor UAVOBJSRCFILENAMES += accelstate UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor +UAVOBJSRCFILENAMES += auxmagsensor UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 0f08a3536..46027b761 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -127,6 +127,7 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/waypointactive.h \ $$UAVOBJECT_SYNTHETICS/mpu6000settings.h \ $$UAVOBJECT_SYNTHETICS/takeofflocation.h \ + $$UAVOBJECT_SYNTHETICS/auxmagsensor.h \ $$UAVOBJECT_SYNTHETICS/perfcounter.h SOURCES += \ @@ -231,5 +232,6 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/waypointactive.cpp \ $$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \ $$UAVOBJECT_SYNTHETICS/takeofflocation.cpp \ + $$UAVOBJECT_SYNTHETICS/auxmagsensor.cpp \ $$UAVOBJECT_SYNTHETICS/perfcounter.cpp diff --git a/shared/uavobjectdefinition/auxmagsensor.xml b/shared/uavobjectdefinition/auxmagsensor.xml new file mode 100644 index 000000000..1deb44da8 --- /dev/null +++ b/shared/uavobjectdefinition/auxmagsensor.xml @@ -0,0 +1,13 @@ + + + Calibrated sensor data from aux 3 axis magnetometer in MilliGauss. + + + + + + + + + + From 9d7a493c26ccb0380828986ecb402efcc236538d Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 17 Jul 2014 20:41:14 +0200 Subject: [PATCH 03/74] OP-1406 - Allows filtermag to switch between onboard and aux mag --- flight/modules/StateEstimation/filtermag.c | 39 +++++++++++++++---- .../StateEstimation/inc/stateestimation.h | 24 ++++++++---- .../modules/StateEstimation/stateestimation.c | 27 ++++++++++++- shared/uavobjectdefinition/magstate.xml | 7 ++-- 4 files changed, 77 insertions(+), 20 deletions(-) diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index c48c4463e..88fdee3de 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -60,7 +60,7 @@ 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 mag[3], bool setAlarms); static void magOffsetEstimation(struct data *this, float mag[3]); @@ -91,8 +91,25 @@ static filterResult filter(stateFilter *self, stateEstimation *state) { struct data *this = (struct data *)self->localdata; + // Uses the external mag when available + if (IS_SET(state->updated, SENSORUPDATES_auxMag) && checkMagValidity(this, state->auxMag, false)) { + state->mag[0] = state->auxMag[0]; + state->mag[1] = state->auxMag[1]; + state->mag[2] = state->auxMag[2]; + state->magStatus = MAGSTATUS_AUX; + return FILTERRESULT_OK; + } + + // Defaults to board magnetometer + state->mag[0] = state->boardMag[0]; + state->mag[1] = state->boardMag[1]; + state->mag[2] = state->boardMag[2]; + state->magStatus = MAGSTATUS_ONBOARD; if (IS_SET(state->updated, SENSORUPDATES_mag)) { - checkMagValidity(this, state->mag); + // set alarm as aux sensor is invalid too + if (!checkMagValidity(this, state->mag, true)) { + state->magStatus = MAGSTATUS_INVALID; + } if (this->revoCalibration.MagBiasNullingRate > 0) { magOffsetEstimation(this, state->mag); } @@ -104,10 +121,10 @@ static filterResult filter(stateFilter *self, stateEstimation *state) /** * check validity of magnetometers */ -static void checkMagValidity(struct data *this, float mag[3]) +static bool checkMagValidity(struct data *this, float mag[3], bool setAlarms) { - #define ALARM_THRESHOLD 5 - + #define ALARM_THRESHOLD 5 + bool valid = true; // mag2 holds the actual magnetic vector length (squared) float mag2 = mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]; @@ -130,21 +147,29 @@ static void checkMagValidity(struct data *this, float mag[3]) if (minWarning2 < mag2 && mag2 < maxWarning2) { this->warningcount = 0; this->errorcount = 0; + valid = true; AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER); } else if (minError2 < mag2 && mag2 < maxError2) { this->errorcount = 0; if (this->warningcount > ALARM_THRESHOLD) { - AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING); + valid = false; + if (setAlarms) { + AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING); + } } else { this->warningcount++; } } else { if (this->errorcount > ALARM_THRESHOLD) { - AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL); + valid = false; + if (setAlarms) { + AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL); + } } else { this->errorcount++; } } + return valid; } diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index d93235f4e..d5a6d35eb 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -47,6 +47,8 @@ typedef enum { SENSORUPDATES_gyro = 1 << 0, SENSORUPDATES_accel = 1 << 1, SENSORUPDATES_mag = 1 << 2, + SENSORUPDATES_auxMag = 1 << 2, + SENSORUPDATES_boardMag = 1 << 2, 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_ONBOARD 1 +#define MAGSTATUS_AUX 2 +#define MAGSTATUS_INVALID 0 typedef struct { - float gyro[3]; - float accel[3]; - float mag[3]; - float attitude[4]; - float pos[3]; - float vel[3]; - float airspeed[2]; - float baro[1]; + float gyro[3]; + float accel[3]; + float mag[3]; + float attitude[4]; + float pos[3]; + float vel[3]; + float airspeed[2]; + float baro[1]; + float auxMag[3]; + uint8_t magStatus; + float boardMag[3]; sensorUpdates updated; } stateEstimation; diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 079db8ca1..2645d2723 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -267,6 +268,7 @@ int32_t StateEstimationInitialize(void) GyroSensorInitialize(); MagSensorInitialize(); + AuxMagSensorInitialize(); BaroSensorInitialize(); AirspeedSensorInitialize(); GPSVelocitySensorInitialize(); @@ -423,7 +425,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 +470,27 @@ 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); + + { + MagStateData s; + + MagStateGet(&s); + s.x = states.mag[0]; + s.y = states.mag[1]; + s.z = states.mag[2]; + switch (states.magStatus) { + case MAGSTATUS_ONBOARD: + 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); diff --git a/shared/uavobjectdefinition/magstate.xml b/shared/uavobjectdefinition/magstate.xml index 1cd0fefaa..0de27ce17 100644 --- a/shared/uavobjectdefinition/magstate.xml +++ b/shared/uavobjectdefinition/magstate.xml @@ -1,9 +1,10 @@ The filtered magnet vector. - - - + + + + From cced294760829a0e2dc5e959d7fc2686e1897dcd Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 17 Jul 2014 21:02:18 +0200 Subject: [PATCH 04/74] OP-1406 - Add support for aux mag calibration settings. todo: - GCS handling for extenal mag calibration --- .../boards/revolution/firmware/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 4 +++- shared/uavobjectdefinition/auxmagcalibration.xml | 13 +++++++++++++ 3 files changed, 17 insertions(+), 1 deletion(-) create mode 100644 shared/uavobjectdefinition/auxmagcalibration.xml diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index c0196527d..47a8f4e92 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -32,6 +32,7 @@ UAVOBJSRCFILENAMES += accelstate UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += auxmagsensor +UAVOBJSRCFILENAMES += auxmagcalibration UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 46027b761..63b0c1adf 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -128,6 +128,7 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/mpu6000settings.h \ $$UAVOBJECT_SYNTHETICS/takeofflocation.h \ $$UAVOBJECT_SYNTHETICS/auxmagsensor.h \ + $$UAVOBJECT_SYNTHETICS/auxmagcalibration.h \ $$UAVOBJECT_SYNTHETICS/perfcounter.h SOURCES += \ @@ -233,5 +234,6 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \ $$UAVOBJECT_SYNTHETICS/takeofflocation.cpp \ $$UAVOBJECT_SYNTHETICS/auxmagsensor.cpp \ + $$UAVOBJECT_SYNTHETICS/auxmagcalibration.cpp \ $$UAVOBJECT_SYNTHETICS/perfcounter.cpp - + diff --git a/shared/uavobjectdefinition/auxmagcalibration.xml b/shared/uavobjectdefinition/auxmagcalibration.xml new file mode 100644 index 000000000..16ff9e88e --- /dev/null +++ b/shared/uavobjectdefinition/auxmagcalibration.xml @@ -0,0 +1,13 @@ + + + Settings for auxiliary magnetometer calibration + + + + + + + + + From 7abbf5fa5a17ae1446f8d4e58b313d7a5d8cb7e0 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 17 Jul 2014 20:44:42 +0200 Subject: [PATCH 05/74] OP-1406 - Fixes for all other targets --- flight/targets/boards/coptercontrol/firmware/Makefile | 1 + flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc | 3 +++ flight/targets/boards/osd/firmware/Makefile | 2 ++ flight/targets/boards/revoproto/firmware/UAVObjects.inc | 3 +++ flight/targets/boards/simposix/firmware/UAVObjects.inc | 2 ++ 5 files changed, 11 insertions(+) diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index 7a5a7e90f..bdcbfaed6 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -128,6 +128,7 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/airspeedstate.c SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c SRC += $(OPUAVSYNTHDIR)/perfcounter.c + SRC += $(OPUAVSYNTHDIR)/auxmagsensor.c else ## Test Code SRC += $(OPTESTS)/test_common.c diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index 49867c39e..ca300f70a 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -31,6 +31,8 @@ UAVOBJSRCFILENAMES += gyrosensor UAVOBJSRCFILENAMES += accelstate UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor +UAVOBJSRCFILENAMES += auxmagsensor +UAVOBJSRCFILENAMES += auxmagcalibration UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor @@ -54,6 +56,7 @@ UAVOBJSRCFILENAMES += gpssatellites UAVOBJSRCFILENAMES += gpstime UAVOBJSRCFILENAMES += gpsvelocitysensor UAVOBJSRCFILENAMES += gpssettings +UAVOBJSRCFILENAMES += gpsextendedstatus UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus UAVOBJSRCFILENAMES += vtolpathfollowersettings diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile index 3744ee56a..6ff4d37cb 100644 --- a/flight/targets/boards/osd/firmware/Makefile +++ b/flight/targets/boards/osd/firmware/Makefile @@ -96,6 +96,8 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/osdsettings.c SRC += $(OPUAVSYNTHDIR)/barosensor.c SRC += $(OPUAVSYNTHDIR)/magsensor.c + SRC += $(OPUAVSYNTHDIR)/auxmagsensor.c + SRC += $(OPUAVSYNTHDIR)/gpsextendedstatus.c else ## Test Code SRC += $(OPTESTS)/test_common.c diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 3905af42f..af830e9da 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -31,6 +31,8 @@ UAVOBJSRCFILENAMES += gyrosensor UAVOBJSRCFILENAMES += accelstate UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor +UAVOBJSRCFILENAMES += auxmagsensor +UAVOBJSRCFILENAMES += auxmagcalibration UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor @@ -54,6 +56,7 @@ UAVOBJSRCFILENAMES += gpssatellites UAVOBJSRCFILENAMES += gpstime UAVOBJSRCFILENAMES += gpsvelocitysensor UAVOBJSRCFILENAMES += gpssettings +UAVOBJSRCFILENAMES += gpsextendedstatus UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus UAVOBJSRCFILENAMES += vtolpathfollowersettings diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 907ff6833..c004fe760 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -36,6 +36,8 @@ UAVOBJSRCFILENAMES += gyrosensor UAVOBJSRCFILENAMES += accelstate UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor +UAVOBJSRCFILENAMES += auxmagsensor +UAVOBJSRCFILENAMES += auxmagcalibration UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor From e511190ad316738260c2d8bf3c2a0c96022683bf Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 17 Jul 2014 20:52:10 +0200 Subject: [PATCH 06/74] OP-1406 - Improve handling of external magnetometer, add additional settings --- flight/libraries/inc/CoordinateConversions.h | 68 ++++++++ flight/modules/StateEstimation/filtermag.c | 154 +++++++++++------- .../StateEstimation/inc/stateestimation.h | 6 +- .../modules/StateEstimation/stateestimation.c | 14 +- .../discoveryf4bare/firmware/UAVObjects.inc | 2 +- .../boards/revolution/firmware/UAVObjects.inc | 2 +- .../boards/revoproto/firmware/UAVObjects.inc | 2 +- .../boards/simposix/firmware/UAVObjects.inc | 2 +- ...xmagcalibration.xml => auxmagsettings.xml} | 5 +- shared/uavobjectdefinition/magstate.xml | 2 +- 10 files changed, 180 insertions(+), 77 deletions(-) rename shared/uavobjectdefinition/{auxmagcalibration.xml => auxmagsettings.xml} (63%) diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h index f64823a43..6ce5650d6 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -112,5 +112,73 @@ inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3]) result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2]; } +inline void matrix_inline_scale_3f(float a[3][3], float scale) +{ + a[0][0] *= scale; + a[0][1] *= scale; + a[0][2] *= scale; + + a[1][0] *= scale; + a[1][1] *= scale; + a[1][2] *= scale; + + a[2][0] *= scale; + a[2][1] *= scale; + a[2][2] *= scale; +} + +inline void rot_about_axis_x(const float rotation, float R[3][3]) +{ + float s = sinf(rotation); + float c = cosf(rotation); + + R[0][0] = 1; + R[0][1] = 0; + R[0][2] = 0; + + R[1][0] = 0; + R[1][1] = c; + R[1][2] = -s; + + R[2][0] = 0; + R[2][1] = s; + R[2][2] = c; +} + +inline void rot_about_axis_y(const float rotation, float R[3][3]) +{ + float s = sinf(rotation); + float c = cosf(rotation); + + R[0][0] = c; + R[0][1] = 0; + R[0][2] = s; + + R[1][0] = 0; + R[1][1] = 1; + R[1][2] = 0; + + R[2][0] = -s; + R[2][1] = 0; + R[2][2] = c; +} + +inline void rot_about_axis_z(const float rotation, float R[3][3]) +{ + float s = sinf(rotation); + float c = cosf(rotation); + + R[0][0] = c; + R[0][1] = -s; + R[0][2] = 0; + + R[1][0] = s; + R[1][1] = c; + R[1][2] = 0; + + R[2][0] = 0; + R[2][1] = 0; + R[2][2] = 1; +} #endif // COORDINATECONVERSIONS_H_ diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index 88fdee3de..6af155365 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -36,7 +36,7 @@ #include #include #include - +#include #include // Private constants @@ -47,10 +47,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 +62,9 @@ struct data { static int32_t init(stateFilter *self); static filterResult filter(stateFilter *self, stateEstimation *state); -static bool checkMagValidity(struct data *this, float mag[3], bool setAlarms); -static void magOffsetEstimation(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,105 +82,131 @@ 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 squared magnetic vector length (extpected) + this->magBe = sqrtf(this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1] + this->homeLocationBe[2] * this->homeLocationBe[2]); + 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; // Uses the external mag when available - if (IS_SET(state->updated, SENSORUPDATES_auxMag) && checkMagValidity(this, state->auxMag, false)) { - state->mag[0] = state->auxMag[0]; - state->mag[1] = state->auxMag[1]; - state->mag[2] = state->auxMag[2]; - state->magStatus = MAGSTATUS_AUX; - return FILTERRESULT_OK; - } - - // Defaults to board magnetometer - state->mag[0] = state->boardMag[0]; - state->mag[1] = state->boardMag[1]; - state->mag[2] = state->boardMag[2]; - state->magStatus = MAGSTATUS_ONBOARD; - if (IS_SET(state->updated, SENSORUPDATES_mag)) { - // set alarm as aux sensor is invalid too - if (!checkMagValidity(this, state->mag, true)) { - state->magStatus = MAGSTATUS_INVALID; - } - if (this->revoCalibration.MagBiasNullingRate > 0) { - magOffsetEstimation(this, state->mag); + 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 + if (checkMagValidity(this, auxMagError, + (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! + /*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) + if (checkMagValidity(this, boardMagError, 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) { + temp_mag[0] *= this->invMagBe; + temp_mag[1] *= this->invMagBe; + temp_mag[2] *= this->invMagBe; + 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 bool checkMagValidity(struct data *this, float mag[3], bool setAlarms) +static bool checkMagValidity(struct data *this, float error, bool setAlarms) { #define ALARM_THRESHOLD 5 - bool valid = true; - // 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() : minlimitmagBe2 * (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); // set errors - if (minWarning2 < mag2 && mag2 < maxWarning2) { + if (error < this->revoSettings.MagnetometerMaxDeviation.Warning) { this->warningcount = 0; this->errorcount = 0; - valid = true; 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) { - valid = false; if (setAlarms) { AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING); } + return false; } else { this->warningcount++; - } - } else { - if (this->errorcount > ALARM_THRESHOLD) { - valid = false; - if (setAlarms) { - AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL); - } - } else { - this->errorcount++; + return true; } } - return valid; + + 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 = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]); + // absolute value of relative error against Be + float error = fabsf(magnitude - this->magBe) * this->invMagBe; + return error; +} +#if 0 /** * 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 @@ -269,7 +297,7 @@ static void magOffsetEstimation(struct data *this, float mag[3]) #endif // if 0 } - +#endif /* if 0 */ /** * @} * @} diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index d5a6d35eb..fd399c818 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -47,8 +47,8 @@ typedef enum { SENSORUPDATES_gyro = 1 << 0, SENSORUPDATES_accel = 1 << 1, SENSORUPDATES_mag = 1 << 2, - SENSORUPDATES_auxMag = 1 << 2, - SENSORUPDATES_boardMag = 1 << 2, + SENSORUPDATES_boardMag = 1 << 10, + SENSORUPDATES_auxMag = 1 << 9, SENSORUPDATES_attitude = 1 << 3, SENSORUPDATES_pos = 1 << 4, SENSORUPDATES_vel = 1 << 5, @@ -57,7 +57,7 @@ typedef enum { SENSORUPDATES_lla = 1 << 8, } sensorUpdates; -#define MAGSTATUS_ONBOARD 1 +#define MAGSTATUS_OK 1 #define MAGSTATUS_AUX 2 #define MAGSTATUS_INVALID 0 typedef struct { diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 2645d2723..3bce31c52 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -292,6 +292,7 @@ int32_t StateEstimationInitialize(void) MagSensorConnectCallback(&sensorUpdatedCb); BaroSensorConnectCallback(&sensorUpdatedCb); AirspeedSensorConnectCallback(&sensorUpdatedCb); + AuxMagSensorConnectCallback(&sensorUpdatedCb); GPSVelocitySensorConnectCallback(&sensorUpdatedCb); GPSPositionSensorConnectCallback(&sensorUpdatedCb); @@ -470,8 +471,7 @@ static void StateEstimationCb(void) gyroDelta[2] = states.gyro[2] - gyroRaw[2]; } EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z); - - { + if (IS_SET(states.updated, SENSORUPDATES_mag)) { MagStateData s; MagStateGet(&s); @@ -479,8 +479,8 @@ static void StateEstimationCb(void) s.y = states.mag[1]; s.z = states.mag[2]; switch (states.magStatus) { - case MAGSTATUS_ONBOARD: - s.Source = MAGSTATE_SOURCE_ONBOARD; + case MAGSTATUS_OK: + s.Source = MAGSTATE_SOURCE_OK; break; case MAGSTATUS_AUX: s.Source = MAGSTATE_SOURCE_AUX; @@ -590,7 +590,11 @@ static void sensorUpdatedCb(UAVObjEvent *ev) } if (ev->obj == MagSensorHandle()) { - updatedSensors |= SENSORUPDATES_mag; + updatedSensors |= SENSORUPDATES_boardMag; + } + + if (ev->obj == AuxMagSensorHandle()) { + updatedSensors |= SENSORUPDATES_auxMag; } if (ev->obj == GPSPositionSensorHandle()) { diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index ca300f70a..60ac23651 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -32,7 +32,7 @@ UAVOBJSRCFILENAMES += accelstate UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += auxmagsensor -UAVOBJSRCFILENAMES += auxmagcalibration +UAVOBJSRCFILENAMES += auxmagsettings UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 47a8f4e92..ac771c9a2 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -32,7 +32,7 @@ UAVOBJSRCFILENAMES += accelstate UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += auxmagsensor -UAVOBJSRCFILENAMES += auxmagcalibration +UAVOBJSRCFILENAMES += auxmagsettings UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index af830e9da..f30d855a2 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -32,7 +32,7 @@ UAVOBJSRCFILENAMES += accelstate UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += auxmagsensor -UAVOBJSRCFILENAMES += auxmagcalibration +UAVOBJSRCFILENAMES += auxmagsettings UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index c004fe760..1498c2e72 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -37,7 +37,7 @@ UAVOBJSRCFILENAMES += accelstate UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += auxmagsensor -UAVOBJSRCFILENAMES += auxmagcalibration +UAVOBJSRCFILENAMES += auxmagsettings UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor diff --git a/shared/uavobjectdefinition/auxmagcalibration.xml b/shared/uavobjectdefinition/auxmagsettings.xml similarity index 63% rename from shared/uavobjectdefinition/auxmagcalibration.xml rename to shared/uavobjectdefinition/auxmagsettings.xml index 16ff9e88e..fda802031 100644 --- a/shared/uavobjectdefinition/auxmagcalibration.xml +++ b/shared/uavobjectdefinition/auxmagsettings.xml @@ -1,10 +1,13 @@ - + Settings for auxiliary magnetometer calibration + + + diff --git a/shared/uavobjectdefinition/magstate.xml b/shared/uavobjectdefinition/magstate.xml index 0de27ce17..49a720fed 100644 --- a/shared/uavobjectdefinition/magstate.xml +++ b/shared/uavobjectdefinition/magstate.xml @@ -4,7 +4,7 @@ - + From 869515b34934ac1cb69b3e0a74dd202269b01190 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 17 Jul 2014 20:53:14 +0200 Subject: [PATCH 07/74] OP-1406 - Enable calibration of external mag from GCS --- .../calibration/sixpointcalibrationmodel.cpp | 236 +++++++++++++----- .../calibration/sixpointcalibrationmodel.h | 25 +- .../src/plugins/uavobjects/uavobjects.pro | 4 +- 3 files changed, 196 insertions(+), 69 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp index 6a9200d8a..afa492f02 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp @@ -29,7 +29,7 @@ #include "extensionsystem/pluginmanager.h" #include "calibration/calibrationuiutils.h" -#include "math.h" +#include #include #include "QDebug" @@ -45,6 +45,7 @@ namespace OpenPilot { SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) : QObject(parent), calibratingMag(false), + externalMagAvailable(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); + externalMagAvailable = 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); + externalMagAvailable = 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 (externalMagAvailable) { + 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 (externalMagAvailable) { + 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 x, QList y, QList 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 (externalMagAvailable) { + 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_R2C2; i++) { + auxCalibrationData.mag_transform[i] = result.auxMagSettingsData.mag_transform[i]; + } + for (int i = 0; i < 3; i++) { + auxCalibrationData.mag_bias[i] = result.auxMagSettingsData.mag_bias[i]; + } + + auxMagSettings->setData(auxCalibrationData); + } } if (calibratingAccel) { AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h index 37c7b7d29..368762e7e 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h @@ -31,10 +31,13 @@ #include "wizardmodel.h" #include "calibration/calibrationutils.h" #include + +#include #include #include #include -#include +#include +#include #include #include @@ -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 externalMagAvailable; bool calibratingAccel; QList calibrationStepsMag; @@ -114,7 +121,6 @@ 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 accel_accum_x; QList accel_accum_y; @@ -122,15 +128,23 @@ public: QList mag_accum_x; QList mag_accum_y; QList mag_accum_z; + QList aux_mag_accum_x; + QList aux_mag_accum_y; + QList aux_mag_accum_z; QList mag_fit_x; QList mag_fit_y; QList mag_fit_z; + QList aux_mag_fit_x; + QList aux_mag_fit_y; + QList 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 +152,7 @@ public: void compute(); void showHelp(QString image); UAVObjectManager *getObjectManager(); + void CalcCalibration(QList x, QList y, QList z, double Be_length, float calibrationMatrix[], float bias[]); }; } diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 63b0c1adf..c01c9d680 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -128,7 +128,7 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/mpu6000settings.h \ $$UAVOBJECT_SYNTHETICS/takeofflocation.h \ $$UAVOBJECT_SYNTHETICS/auxmagsensor.h \ - $$UAVOBJECT_SYNTHETICS/auxmagcalibration.h \ + $$UAVOBJECT_SYNTHETICS/auxmagsettings.h \ $$UAVOBJECT_SYNTHETICS/perfcounter.h SOURCES += \ @@ -234,6 +234,6 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \ $$UAVOBJECT_SYNTHETICS/takeofflocation.cpp \ $$UAVOBJECT_SYNTHETICS/auxmagsensor.cpp \ - $$UAVOBJECT_SYNTHETICS/auxmagcalibration.cpp \ + $$UAVOBJECT_SYNTHETICS/auxmagsettings.cpp \ $$UAVOBJECT_SYNTHETICS/perfcounter.cpp From 84758a67311bb8dfa3f147d53ea3a335434576ac Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 3 Aug 2014 11:40:55 +0200 Subject: [PATCH 08/74] OP-1406 - Always feeds mag data even if invalid --- flight/modules/StateEstimation/filtermag.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index 6af155365..28dafd8cc 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -105,8 +105,9 @@ static filterResult filter(stateFilter *self, stateEstimation *state) IS_SET(state->updated, SENSORUPDATES_auxMag)) { auxMagError = getMagError(this, state->auxMag); // Handles alarms only if it will rely on aux mag only - if (checkMagValidity(this, auxMagError, - (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY))) { + 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]; @@ -123,7 +124,10 @@ static filterResult filter(stateFilter *self, stateEstimation *state) }*/ boardMagError = getMagError(this, state->boardMag); // sets warning only if no mag data are available (aux is invalid or missing) - if (checkMagValidity(this, boardMagError, temp_status == MAGSTATUS_INVALID)) { + 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]; From 83a3822798f100ec54552254074b8b19d8eba653 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 3 Aug 2014 11:41:25 +0200 Subject: [PATCH 09/74] OP-1406 - Reenable mag offset estimation for onboard mag only right now --- flight/modules/StateEstimation/filtermag.c | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index 28dafd8cc..a4d9c24bb 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -63,7 +63,7 @@ struct data { static int32_t init(stateFilter *self); static filterResult filter(stateFilter *self, stateEstimation *state); static bool checkMagValidity(struct data *this, float error, bool setAlarms); -// static void magOffsetEstimation(struct data *this, float mag[3]); +static void magOffsetEstimation(struct data *this, float mag[3]); static float getMagError(struct data *this, float mag[3]); int32_t filterMagInitialize(stateFilter *handle) @@ -118,10 +118,10 @@ static filterResult filter(stateFilter *self, stateEstimation *state) if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_AUXONLY) && IS_SET(state->updated, SENSORUPDATES_boardMag)) { - // TODO! - /*if (this->revoCalibration.MagBiasNullingRate > 0) { + // 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)); @@ -204,7 +204,7 @@ static float getMagError(struct data *this, float mag[3]) return error; } -#if 0 + /** * Perform an update of the @ref MagBias based on * Magmeter Offset Cancellation: Theory and Implementation, @@ -301,7 +301,6 @@ void magOffsetEstimation(struct data *this, float mag[3]) #endif // if 0 } -#endif /* if 0 */ /** * @} * @} From 55557ecd607dbcb4175997d205e4aa186e0e767b Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 17 Jul 2014 20:57:06 +0200 Subject: [PATCH 10/74] OP-1434 - Add gps extended status --- .../boards/revolution/firmware/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 ++ shared/uavobjectdefinition/gpsextendedstatus.xml | 15 +++++++++++++++ 3 files changed, 18 insertions(+) create mode 100644 shared/uavobjectdefinition/gpsextendedstatus.xml diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index ac771c9a2..60ac23651 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -56,6 +56,7 @@ UAVOBJSRCFILENAMES += gpssatellites UAVOBJSRCFILENAMES += gpstime UAVOBJSRCFILENAMES += gpsvelocitysensor UAVOBJSRCFILENAMES += gpssettings +UAVOBJSRCFILENAMES += gpsextendedstatus UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus UAVOBJSRCFILENAMES += vtolpathfollowersettings diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index c01c9d680..ecf662715 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -129,6 +129,7 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/takeofflocation.h \ $$UAVOBJECT_SYNTHETICS/auxmagsensor.h \ $$UAVOBJECT_SYNTHETICS/auxmagsettings.h \ + $$UAVOBJECT_SYNTHETICS/gpsextendedstatus.h \ $$UAVOBJECT_SYNTHETICS/perfcounter.h SOURCES += \ @@ -235,5 +236,6 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/takeofflocation.cpp \ $$UAVOBJECT_SYNTHETICS/auxmagsensor.cpp \ $$UAVOBJECT_SYNTHETICS/auxmagsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/gpsextendedstatus.cpp \ $$UAVOBJECT_SYNTHETICS/perfcounter.cpp diff --git a/shared/uavobjectdefinition/gpsextendedstatus.xml b/shared/uavobjectdefinition/gpsextendedstatus.xml new file mode 100644 index 000000000..d5da25352 --- /dev/null +++ b/shared/uavobjectdefinition/gpsextendedstatus.xml @@ -0,0 +1,15 @@ + + + Extended GPS status. + + + + + + + + + + + + From 397bfdc08b54afb6ee04c5e181f71a2a59a79d9a Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 17 Jul 2014 20:39:47 +0200 Subject: [PATCH 11/74] OP-1434 - Add support for OP custom sentences on flight board --- flight/modules/GPS/GPS.c | 4 +++- flight/modules/GPS/UBX.c | 34 ++++++++++++++++++++++++++++++++++ flight/modules/GPS/inc/UBX.h | 26 +++++++++++++++++++++++++- 3 files changed, 62 insertions(+), 2 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 382f1ba00..eafcb2974 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -40,7 +40,7 @@ #include "gpssettings.h" #include "taskinfo.h" #include "hwsettings.h" - +#include "auxmagsensor.h" #include "WorldMagModel.h" #include "CoordinateConversions.h" @@ -153,6 +153,8 @@ int32_t GPSInitialize(void) GPSTimeInitialize(); GPSSatellitesInitialize(); HomeLocationInitialize(); + AuxMagSensorInitialize(); + GPSExtendedStatusInitialize(); updateSettings(); #else diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 86910ef0d..57ff18ffa 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -293,6 +293,30 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi } #endif } + +void parse_ubx_op_sys(struct UBX_OP_SYSINFO *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); +} +void parse_ubx_op_mag(struct UBX_OP_MAG *mag) +{ + AuxMagSensorData data; + + data.x = mag->x; + data.y = mag->y; + data.z = mag->z; + data.Status = mag->Status; + AuxMagSensorSet(&data); +} + #if !defined(PIOS_GPS_MINIMAL) void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc) { @@ -401,6 +425,16 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi #endif } break; + case UBX_CLASS_OP_CUST: + switch (ubx->header.id) { + case UBX_ID_SYS: + parse_ubx_op_sys(&ubx->payload.op_sysinfo); + break; + case UBX_ID_MAG: + parse_ubx_op_mag(&ubx->payload.op_mag); + break; + } + break; } if (msgtracker.msg_received == ALL_RECEIVED) { diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index f32a267bc..a4116e443 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -32,6 +32,8 @@ #define UBX_H #include "openpilot.h" #include "gpspositionsensor.h" +#include "gpsextendedstatus.h" +#include "auxmagsensor.h" #include "GPS.h" @@ -41,7 +43,8 @@ // From u-blox6 receiver protocol specification // Messages classes -#define UBX_CLASS_NAV 0x01 +#define UBX_CLASS_NAV 0x01 +#define UBX_CLASS_OP_CUST 0x99 // Message IDs #define UBX_ID_POSLLH 0x02 @@ -53,6 +56,8 @@ #define UBX_ID_SVINFO 0x30 #define UBX_ID_PVT 0x07 +#define UBX_ID_SYS 0x01 +#define UBX_ID_MAG 0x02 // private structures // Geodetic Position Solution @@ -245,6 +250,23 @@ struct UBX_NAV_SVINFO { struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times }; +// OP custom messages +struct UBX_OP_SYSINFO { + uint32_t flightTime; + uint32_t HeapRemaining; + uint16_t IRQStackRemaining; + uint16_t SystemModStackRemaining; + uint16_t options; +}; + +// OP custom messages +struct UBX_OP_MAG { + uint16_t x; + uint16_t y; + uint16_t z; + uint16_t Status; +}; + typedef union { uint8_t payload[0]; struct UBX_NAV_POSLLH nav_posllh; @@ -257,6 +279,8 @@ typedef union { struct UBX_NAV_TIMEUTC nav_timeutc; struct UBX_NAV_SVINFO nav_svinfo; #endif + struct UBX_OP_SYSINFO op_sysinfo; + struct UBX_OP_MAG op_mag; } UBXPayload; struct UBXHeader { From ce1e4e728eb25c885b6e529588e7e9b5c285336d Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 17 Jul 2014 20:43:16 +0200 Subject: [PATCH 12/74] OP-1434 - Support for mag calibration --- flight/modules/GPS/GPS.c | 19 ++++++++++++++++--- flight/modules/GPS/UBX.c | 33 ++++++++++++++++++++++++++++----- flight/modules/GPS/inc/GPS.h | 1 + flight/modules/GPS/inc/UBX.h | 7 ++++--- 4 files changed, 49 insertions(+), 11 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index eafcb2974..632c3aea6 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -98,7 +98,9 @@ static uint32_t timeOfLastUpdateMs; #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) static struct GPS_RX_STATS gpsRxStats; #endif - +#ifdef PIOS_INCLUDE_GPS_UBX_PARSER +void AuxMagCalibrationUpdatedCb(UAVObjEvent *ev); +#endif // **************** /** * Initialise the gps module @@ -153,10 +155,16 @@ int32_t GPSInitialize(void) GPSTimeInitialize(); GPSSatellitesInitialize(); HomeLocationInitialize(); +#ifdef PIOS_INCLUDE_GPS_UBX_PARSER AuxMagSensorInitialize(); + AuxMagCalibrationInitialize(); GPSExtendedStatusInitialize(); - updateSettings(); + // Initialize mag parameters + AuxMagCalibrationUpdatedCb(NULL); + AuxMagCalibrationConnectCallback(AuxMagCalibrationUpdatedCb); +#endif + updateSettings(); #else if (gpsPort && gpsEnabled) { GPSPositionSensorInitialize(); @@ -381,7 +389,12 @@ static void updateSettings() } } } - +#ifdef PIOS_INCLUDE_GPS_UBX_PARSER +void AuxMagCalibrationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + load_mag_settings(); +} +#endif /** * @} * @} diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 57ff18ffa..32d03b870 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -34,6 +34,13 @@ #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) #include "inc/UBX.h" #include "inc/GPS.h" +#include "CoordinateConversions.h" +#include "auxmagcalibration.h" + +static float mag_bias[3] = { 0, 0, 0 }; +static float mag_transform[3][3] = { + { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } +}; // 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) @@ -293,7 +300,7 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi } #endif } - +#if !defined(PIOS_GPS_MINIMAL) void parse_ubx_op_sys(struct UBX_OP_SYSINFO *sysinfo) { GPSExtendedStatusData data; @@ -306,13 +313,21 @@ void parse_ubx_op_sys(struct UBX_OP_SYSINFO *sysinfo) data.Status = GPSEXTENDEDSTATUS_STATUS_GPSV9; GPSExtendedStatusSet(&data); } +#endif void parse_ubx_op_mag(struct UBX_OP_MAG *mag) { - AuxMagSensorData data; + float mags[3] = { mag->x - mag_bias[0], + mag->y - mag_bias[1], + mag->z - mag_bias[2] }; - data.x = mag->x; - data.y = mag->y; - data.z = mag->z; + float mag_out[3]; + + 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 = mag->Status; AuxMagSensorSet(&data); } @@ -427,9 +442,11 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi break; case UBX_CLASS_OP_CUST: switch (ubx->header.id) { +#if !defined(PIOS_GPS_MINIMAL) case UBX_ID_SYS: parse_ubx_op_sys(&ubx->payload.op_sysinfo); break; +#endif case UBX_ID_MAG: parse_ubx_op_mag(&ubx->payload.op_mag); break; @@ -445,4 +462,10 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi return id; } + +void load_mag_settings() +{ + AuxMagCalibrationmag_transformArrayGet((float *)mag_transform); + AuxMagCalibrationmag_biasArrayGet(mag_bias); +} #endif // PIOS_INCLUDE_GPS_UBX_PARSER diff --git a/flight/modules/GPS/inc/GPS.h b/flight/modules/GPS/inc/GPS.h index 6cecaa845..d88bcff1a 100644 --- a/flight/modules/GPS/inc/GPS.h +++ b/flight/modules/GPS/inc/GPS.h @@ -38,6 +38,7 @@ #include "gpssatellites.h" #include "gpspositionsensor.h" #include "gpstime.h" +#include "auxmagcalibration.h" #define NO_PARSER -3 // no parser available #define PARSER_OVERRUN -2 // message buffer overrun before completing the message diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index a4116e443..1393897e1 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -261,9 +261,9 @@ struct UBX_OP_SYSINFO { // OP custom messages struct UBX_OP_MAG { - uint16_t x; - uint16_t y; - uint16_t z; + int16_t x; + int16_t y; + int16_t z; uint16_t Status; }; @@ -299,5 +299,6 @@ struct UBXPacket { 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 */ From 0198129a1ce85de61e6560486d002af1f85e4665 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 5 Jul 2014 19:52:29 +0200 Subject: [PATCH 13/74] OP-1434 - Add 230400 to GPS port speed list --- flight/modules/GPS/GPS.c | 3 +++ shared/uavobjectdefinition/hwsettings.xml | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 632c3aea6..4c0e4c2e7 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -386,6 +386,9 @@ static void updateSettings() case HWSETTINGS_GPSSPEED_115200: PIOS_COM_ChangeBaud(gpsPort, 115200); break; + case HWSETTINGS_GPSSPEED_230400: + PIOS_COM_ChangeBaud(gpsPort, 230400); + break; } } } diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index c41187009..856e66073 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -17,7 +17,7 @@ - + From 8df34b01d74d9fea630f29deb327f4b53869dbe4 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 3 Aug 2014 16:05:47 +0200 Subject: [PATCH 14/74] OP-1434 - Fix heapremaining data type --- flight/modules/GPS/inc/UBX.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 1393897e1..c00033ed2 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -253,7 +253,7 @@ struct UBX_NAV_SVINFO { // OP custom messages struct UBX_OP_SYSINFO { uint32_t flightTime; - uint32_t HeapRemaining; + uint16_t HeapRemaining; uint16_t IRQStackRemaining; uint16_t SystemModStackRemaining; uint16_t options; From 85be5ba7af74d13bf90e2721cfddbcb66787d777 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 20 Aug 2014 21:05:56 +0200 Subject: [PATCH 15/74] OP-1434 - handle mag rotation --- flight/modules/GPS/GPS.c | 10 +++++----- flight/modules/GPS/UBX.c | 27 ++++++++++++++++++++++++--- flight/modules/GPS/inc/GPS.h | 2 +- 3 files changed, 30 insertions(+), 9 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 4c0e4c2e7..29635f168 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -99,7 +99,7 @@ static uint32_t timeOfLastUpdateMs; static struct GPS_RX_STATS gpsRxStats; #endif #ifdef PIOS_INCLUDE_GPS_UBX_PARSER -void AuxMagCalibrationUpdatedCb(UAVObjEvent *ev); +void AuxMagSettingsUpdatedCb(UAVObjEvent *ev); #endif // **************** /** @@ -157,12 +157,12 @@ int32_t GPSInitialize(void) HomeLocationInitialize(); #ifdef PIOS_INCLUDE_GPS_UBX_PARSER AuxMagSensorInitialize(); - AuxMagCalibrationInitialize(); + AuxMagSettingsInitialize(); GPSExtendedStatusInitialize(); // Initialize mag parameters - AuxMagCalibrationUpdatedCb(NULL); - AuxMagCalibrationConnectCallback(AuxMagCalibrationUpdatedCb); + AuxMagSettingsUpdatedCb(NULL); + AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb); #endif updateSettings(); #else @@ -393,7 +393,7 @@ static void updateSettings() } } #ifdef PIOS_INCLUDE_GPS_UBX_PARSER -void AuxMagCalibrationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { load_mag_settings(); } diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 32d03b870..4bc8255c2 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -35,12 +35,13 @@ #include "inc/UBX.h" #include "inc/GPS.h" #include "CoordinateConversions.h" -#include "auxmagcalibration.h" +#include "auxmagsettings.h" static float mag_bias[3] = { 0, 0, 0 }; static float mag_transform[3][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } }; +static bool useMag = false; // 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) @@ -316,6 +317,9 @@ void parse_ubx_op_sys(struct UBX_OP_SYSINFO *sysinfo) #endif void parse_ubx_op_mag(struct UBX_OP_MAG *mag) { + if (!useMag) { + return; + } float mags[3] = { mag->x - mag_bias[0], mag->y - mag_bias[1], mag->z - mag_bias[2] }; @@ -465,7 +469,24 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi void load_mag_settings() { - AuxMagCalibrationmag_transformArrayGet((float *)mag_transform); - AuxMagCalibrationmag_biasArrayGet(mag_bias); + AuxMagSettingsTypeOptions option; + + AuxMagSettingsTypeGet(&option); + if (option != 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 { + float a[3][3]; + float b[3][3]; + float rotz; + AuxMagSettingsmag_transformArrayGet((float *)a); + AuxMagSettingsOrientationGet(&rotz); + rot_about_axis_z(rotz, b); + matrix_mult_3x3f(a, b, mag_transform); + AuxMagSettingsmag_biasArrayGet(mag_bias); + useMag = true; + } } #endif // PIOS_INCLUDE_GPS_UBX_PARSER diff --git a/flight/modules/GPS/inc/GPS.h b/flight/modules/GPS/inc/GPS.h index d88bcff1a..c952db4cb 100644 --- a/flight/modules/GPS/inc/GPS.h +++ b/flight/modules/GPS/inc/GPS.h @@ -38,7 +38,7 @@ #include "gpssatellites.h" #include "gpspositionsensor.h" #include "gpstime.h" -#include "auxmagcalibration.h" +#include "auxmagsettings.h" #define NO_PARSER -3 // no parser available #define PARSER_OVERRUN -2 // message buffer overrun before completing the message From b6c053565e33c99122daab178c0c05159d1820b6 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 7 Aug 2014 14:34:43 +0200 Subject: [PATCH 16/74] OP-1434 - use degrees for rotation, fix Calibration gui --- flight/modules/GPS/UBX.c | 5 +++-- .../plugins/config/calibration/sixpointcalibrationmodel.cpp | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 4bc8255c2..414cdae0f 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -30,11 +30,11 @@ #include "openpilot.h" #include "pios.h" - +#include "CoordinateConversions.h" +#include "pios_math.h" #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) #include "inc/UBX.h" #include "inc/GPS.h" -#include "CoordinateConversions.h" #include "auxmagsettings.h" static float mag_bias[3] = { 0, 0, 0 }; @@ -483,6 +483,7 @@ void load_mag_settings() 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); diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp index afa492f02..526a2a9a4 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp @@ -592,7 +592,7 @@ void SixPointCalibrationModel::save() if (externalMagAvailable) { 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_R2C2; i++) { + 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++) { From 002c35163ae2fcde32ba81eb95a349155daad8ae Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 21 Aug 2014 16:55:16 +0200 Subject: [PATCH 17/74] OP-1370 - Add support for mon-ver ack-ack and ack-nak messages for gps hw version detection --- flight/modules/GPS/NMEA.c | 2 +- flight/modules/GPS/UBX.c | 110 +++++++++++++----- flight/modules/GPS/inc/UBX.h | 81 ++++++++++--- .../uavobjectdefinition/gpspositionsensor.xml | 1 + 4 files changed, 148 insertions(+), 46 deletions(-) diff --git a/flight/modules/GPS/NMEA.c b/flight/modules/GPS/NMEA.c index 7aac3b5c2..e45702990 100644 --- a/flight/modules/GPS/NMEA.c +++ b/flight/modules/GPS/NMEA.c @@ -489,7 +489,7 @@ static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdate // geoid separation GpsData->GeoidSeparation = NMEA_real_to_float(param[11]); - + GpsData->SensorType = GPSPOSITIONSENSOR_SENSORTYPE_NMEA; return true; } diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 414cdae0f..babca4533 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -36,12 +36,17 @@ #include "inc/UBX.h" #include "inc/GPS.h" #include "auxmagsettings.h" +#include static float mag_bias[3] = { 0, 0, 0 }; static float mag_transform[3][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } }; static bool useMag = false; +static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; +static struct UBX_ACK_ACK last_ack; +static struct UBX_ACK_NAK last_nak; +static bool usePvt = false; // 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) @@ -204,6 +209,9 @@ bool checksum_ubx_message(struct UBXPacket *ubx) void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionSensorData *GpsPosition) { + if (usePvt) { + return; + } if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) { if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) { GpsPosition->Altitude = (float)posllh->hMSL * 0.001f; @@ -216,6 +224,9 @@ void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionSensorData * void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionSensorData *GpsPosition) { + if (usePvt) { + return; + } if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) { GpsPosition->Satellites = sol->numSV; @@ -246,6 +257,9 @@ void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionSensorData *GpsPositi void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *GpsPosition) { + if (usePvt) { + return; + } GPSVelocitySensorData GpsVelocity; if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) { @@ -300,7 +314,28 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi GPSTimeSet(&GpsTime); } #endif + GpsPosition->SensorType = sensorType; } + +void parse_ubx_ack_ack(struct UBX_ACK_ACK *ack_ack) +{ + last_ack = *ack_ack; +} + +void parse_ubx_ack_nak(struct UBX_ACK_NAK *ack_nak) +{ + last_nak = *ack_nak; +} + +void parse_ubx_mon_ver(struct UBX_MON_VER *mon_ver) +{ + uint32_t hwVersion = atoi(mon_ver->hwVersion); + + sensorType = (hwVersion >= 8000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 : + ((hwVersion >= 7000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX); +} + + #if !defined(PIOS_GPS_MINIMAL) void parse_ubx_op_sys(struct UBX_OP_SYSINFO *sysinfo) { @@ -339,6 +374,9 @@ void parse_ubx_op_mag(struct UBX_OP_MAG *mag) #if !defined(PIOS_GPS_MINIMAL) void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc) { + if (usePvt) { + return; + } // Test if time is valid if ((timeutc->valid & TIMEUTC_VALIDTOW) && (timeutc->valid & TIMEUTC_VALIDWKN)) { // Time is valid, set GpsTime @@ -404,54 +442,70 @@ 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); + usePvt = (lastPvtTime) && (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000); - switch (ubx->header.class) { + switch ((ubx_class)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; + // Set of messages to be decoded when not using pvt + switch ((ubx_class_nav_id)ubx->header.id) { + case UBX_ID_NAV_POSLLH: + parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition); + break; + case UBX_ID_NAV_SOL: + parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition); + break; + case UBX_ID_NAV_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; + case UBX_ID_NAV_TIMEUTC: + parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc); + break; #endif - } - } - // messages used always - switch (ubx->header.id) { - case UBX_ID_DOP: + case UBX_ID_NAV_DOP: parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition); break; - - case UBX_ID_PVT: + case UBX_ID_NAV_PVT: parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition); lastPvtTime = PIOS_DELAY_GetuS(); break; #if !defined(PIOS_GPS_MINIMAL) - case UBX_ID_SVINFO: + case UBX_ID_NAV_SVINFO: parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo); break; #endif + default: + break; + } + break; + case UBX_CLASS_MON: + switch ((ubx_class_mon_id)ubx->header.id) { + case UBX_ID_MON_VER: + parse_ubx_mon_ver(&ubx->payload.mon_ver); + break; + default: + break; + } + case UBX_CLASS_ACK: + switch ((ubx_class_ack_id)ubx->header.id) { + case UBX_ID_ACK_ACK: + parse_ubx_ack_ack(&ubx->payload.ack_ack); + break; + case UBX_ID_ACK_NAK: + parse_ubx_ack_nak(&ubx->payload.ack_nak); + break; + default: + break; } break; case UBX_CLASS_OP_CUST: - switch (ubx->header.id) { + switch ((ubx_class_op_id)ubx->header.id) { #if !defined(PIOS_GPS_MINIMAL) - case UBX_ID_SYS: + case UBX_ID_OP_SYS: parse_ubx_op_sys(&ubx->payload.op_sysinfo); break; #endif - case UBX_ID_MAG: + case UBX_ID_OP_MAG: parse_ubx_op_mag(&ubx->payload.op_mag); break; } diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index c00033ed2..3a44f369c 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -37,27 +37,45 @@ #include "GPS.h" -#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 -#define UBX_CLASS_OP_CUST 0x99 +typedef enum { + UBX_CLASS_NAV = 0x01, + UBX_CLASS_ACK = 0x05, + UBX_CLASS_MON = 0x0A, + UBX_CLASS_OP_CUST = 0x99, +} 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_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, +} ubx_class_mon_id; + +typedef enum { + UBX_ID_ACK_ACK = 0x01, + UBX_ID_ACK_NAK = 0x00, +} ubx_class_ack_id; -#define UBX_ID_SYS 0x01 -#define UBX_ID_MAG 0x02 // private structures // Geodetic Position Solution @@ -250,6 +268,29 @@ 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 1 +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; @@ -261,14 +302,15 @@ struct UBX_OP_SYSINFO { // OP custom messages struct UBX_OP_MAG { - int16_t x; - int16_t y; - int16_t z; + 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; @@ -279,6 +321,11 @@ 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; diff --git a/shared/uavobjectdefinition/gpspositionsensor.xml b/shared/uavobjectdefinition/gpspositionsensor.xml index cfaba9db0..ced2c027a 100644 --- a/shared/uavobjectdefinition/gpspositionsensor.xml +++ b/shared/uavobjectdefinition/gpspositionsensor.xml @@ -12,6 +12,7 @@ + From c6a82c5d2382314312aef6f99f2ad7f8d5ac5100 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 21 Aug 2014 17:35:17 +0200 Subject: [PATCH 18/74] OP-1370 - refactors UBX code to use a table of messages and handler instead of neverending switches --- flight/modules/GPS/UBX.c | 265 ++++++++++++++++++++------------------- 1 file changed, 134 insertions(+), 131 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index babca4533..f08855854 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -32,6 +32,7 @@ #include "pios.h" #include "CoordinateConversions.h" #include "pios_math.h" +#include #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) #include "inc/UBX.h" #include "inc/GPS.h" @@ -47,9 +48,55 @@ static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORT static struct UBX_ACK_ACK last_ack; static struct UBX_ACK_NAK last_nak; static bool usePvt = false; +static uint32_t lastPvtTime = 0; +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) // 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) +#define UBX_PVT_TIMEOUT (1000) // parse incoming character stream for messages in UBX binary format int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats) @@ -207,11 +254,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; @@ -222,11 +271,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; @@ -246,8 +296,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; @@ -255,13 +307,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; @@ -274,10 +326,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; @@ -317,66 +371,14 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi GpsPosition->SensorType = sensorType; } -void parse_ubx_ack_ack(struct UBX_ACK_ACK *ack_ack) -{ - last_ack = *ack_ack; -} - -void parse_ubx_ack_nak(struct UBX_ACK_NAK *ack_nak) -{ - last_nak = *ack_nak; -} - -void parse_ubx_mon_ver(struct UBX_MON_VER *mon_ver) -{ - uint32_t hwVersion = atoi(mon_ver->hwVersion); - - sensorType = (hwVersion >= 8000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 : - ((hwVersion >= 7000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX); -} - - #if !defined(PIOS_GPS_MINIMAL) -void parse_ubx_op_sys(struct UBX_OP_SYSINFO *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 -void parse_ubx_op_mag(struct UBX_OP_MAG *mag) -{ - if (!useMag) { - return; - } - float mags[3] = { mag->x - mag_bias[0], - mag->y - mag_bias[1], - mag->z - mag_bias[2] }; - - float mag_out[3]; - - 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 = mag->Status; - AuxMagSensorSet(&data); -} - -#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 @@ -398,10 +400,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++) { @@ -425,14 +428,75 @@ 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; + + last_ack = *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; + + last_nak = *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; + uint32_t hwVersion = atoi(mon_ver->hwVersion); + + sensorType = (hwVersion >= 8000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 : + ((hwVersion >= 7000) ? 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_bias[0], + mag->y - mag_bias[1], + mag->z - mag_bias[2] }; + + float mag_out[3]; + + 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 = mag->Status; + AuxMagSensorSet(&data); +} + + // 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. @@ -443,73 +507,12 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi } // is it using PVT? usePvt = (lastPvtTime) && (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000); - - switch ((ubx_class)ubx->header.class) { - case UBX_CLASS_NAV: - // Set of messages to be decoded when not using pvt - switch ((ubx_class_nav_id)ubx->header.id) { - case UBX_ID_NAV_POSLLH: - parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition); - break; - case UBX_ID_NAV_SOL: - parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition); - break; - case UBX_ID_NAV_VELNED: - parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition); - break; -#if !defined(PIOS_GPS_MINIMAL) - case UBX_ID_NAV_TIMEUTC: - parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc); - break; -#endif - case UBX_ID_NAV_DOP: - parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition); - break; - case UBX_ID_NAV_PVT: - parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition); - lastPvtTime = PIOS_DELAY_GetuS(); - break; -#if !defined(PIOS_GPS_MINIMAL) - case UBX_ID_NAV_SVINFO: - parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo); - break; -#endif - default: + 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; } - break; - case UBX_CLASS_MON: - switch ((ubx_class_mon_id)ubx->header.id) { - case UBX_ID_MON_VER: - parse_ubx_mon_ver(&ubx->payload.mon_ver); - break; - default: - break; - } - case UBX_CLASS_ACK: - switch ((ubx_class_ack_id)ubx->header.id) { - case UBX_ID_ACK_ACK: - parse_ubx_ack_ack(&ubx->payload.ack_ack); - break; - case UBX_ID_ACK_NAK: - parse_ubx_ack_nak(&ubx->payload.ack_nak); - break; - default: - break; - } - break; - case UBX_CLASS_OP_CUST: - switch ((ubx_class_op_id)ubx->header.id) { -#if !defined(PIOS_GPS_MINIMAL) - case UBX_ID_OP_SYS: - parse_ubx_op_sys(&ubx->payload.op_sysinfo); - break; -#endif - case UBX_ID_OP_MAG: - parse_ubx_op_mag(&ubx->payload.op_mag); - break; - } - break; } if (msgtracker.msg_received == ALL_RECEIVED) { From e64ab1f1ff066da4794fee5d935bf8efcb7a2135 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 22 Aug 2014 02:41:36 +0200 Subject: [PATCH 19/74] OP-1370 - Configure bidirectional COM port for GPS --- flight/targets/boards/revolution/firmware/pios_board.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index b26f75a8c..e029b90dc 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -217,6 +217,7 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; #define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 #define PIOS_COM_GPS_RX_BUF_LEN 32 +#define PIOS_COM_GPS_TX_BUF_LEN 32 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 @@ -609,7 +610,7 @@ void PIOS_Board_Init(void) PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWSETTINGS_RM_MAINPORT_GPS: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); + PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); break; case HWSETTINGS_RM_MAINPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) @@ -699,7 +700,7 @@ void PIOS_Board_Init(void) #endif /* PIOS_INCLUDE_I2C */ break; case HWSETTINGS_RM_FLEXIPORT_GPS: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); break; case HWSETTINGS_RM_FLEXIPORT_DSM2: case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: From 903a77669e742ad41a81fc97d81571e324f9a417 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 22 Aug 2014 02:43:30 +0200 Subject: [PATCH 20/74] OP-1370 - Add "management tasks" support for UBX, used for self configuration. Initial state machine asking for gps version. Configuration messages definition --- flight/modules/GPS/GPS.c | 13 +++++- flight/modules/GPS/UBX.c | 87 ++++++++++++++++++++++++++++++++++-- flight/modules/GPS/inc/UBX.h | 73 ++++++++++++++++++++++++++++-- 3 files changed, 165 insertions(+), 8 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 29635f168..d191ac8a0 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -43,6 +43,7 @@ #include "auxmagsensor.h" #include "WorldMagModel.h" #include "CoordinateConversions.h" +#include #include "GPS.h" #include "NMEA.h" @@ -228,7 +229,17 @@ static void gpsTask(__attribute__((unused)) void *parameters) // Loop forever while (1) { uint8_t c; - +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) + if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) { + char *buffer = 0; + uint16_t count = 0; + ubx_run_management_tasks(&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; diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index f08855854..0bc4fb26c 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -33,6 +33,7 @@ #include "CoordinateConversions.h" #include "pios_math.h" #include +#include #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) #include "inc/UBX.h" #include "inc/GPS.h" @@ -43,12 +44,16 @@ static float mag_bias[3] = { 0, 0, 0 }; static float mag_transform[3][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } }; + static bool useMag = false; static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; static struct UBX_ACK_ACK last_ack; static struct UBX_ACK_NAK last_nak; static bool usePvt = false; static uint32_t lastPvtTime = 0; + + +// parse table item typedef struct { uint8_t msgClass; uint8_t msgID; @@ -95,8 +100,23 @@ const ubx_message_handler ubx_handler_table[] = { { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_VER, .handler = &parse_ubx_mon_ver }, }; #define UBX_HANDLER_TABLE_SIZE NELEMENTS(ubx_handler_table) + +// COnfiguration support +typedef enum { + INIT_STEP_ASK_VER = 0, + INIT_STEP_WAIT_VER = 1, + INIT_STEP_CONFIGURE = 2, + INIT_STEP_DONE = 3, +} initSteps; + +static initSteps currentConfigurationStep; +// timestamp of last operation +static uint32_t lastStepTimestampRaw = 0; +// detected hw version +static int32_t hwVersion = -1; + // 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) +#define UBX_PVT_TIMEOUT (1000) // parse incoming character stream for messages in UBX binary format int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats) @@ -445,10 +465,11 @@ static void parse_ubx_ack_nak(struct UBXPacket *ubx, __attribute__((unused)) GPS static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition) { struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver; - uint32_t hwVersion = atoi(mon_ver->hwVersion); - sensorType = (hwVersion >= 8000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 : - ((hwVersion >= 7000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX); + hwVersion = atoi(mon_ver->hwVersion); + + sensorType = (hwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 : + ((hwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX); } @@ -547,4 +568,62 @@ void load_mag_settings() useMag = true; } } + +UBXSentPacket_t working_packet; +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; +} +void 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); +} + +void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *count) +{ + prepare_packet(packet, classID, messageID, 0); + *count = packet->message.header.len + sizeof(UBXSentHeader_t) + 2; +} + +void ubx_run_management_tasks(char * *buffer, uint16_t *count) +{ + uint32_t elapsed = PIOS_DELAY_DiffuS(lastStepTimestampRaw); + + switch (currentConfigurationStep) { + case INIT_STEP_ASK_VER: + lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + build_request(&working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, count); + *buffer = (char *)working_packet.buffer; + currentConfigurationStep = INIT_STEP_WAIT_VER; + break; + case INIT_STEP_WAIT_VER: + if (hwVersion > 0) { + currentConfigurationStep = INIT_STEP_CONFIGURE; + } else if (elapsed > UBX_REPLY_TIMEOUT) { + currentConfigurationStep = INIT_STEP_ASK_VER; + } + return; + + case INIT_STEP_CONFIGURE: + case INIT_STEP_DONE: + break; + } +} + #endif // PIOS_INCLUDE_GPS_UBX_PARSER diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 3a44f369c..973bc1bd6 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -36,9 +36,10 @@ #include "auxmagsensor.h" #include "GPS.h" +#define UBX_REPLY_TIMEOUT (500 * 1000) -#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 @@ -46,6 +47,7 @@ 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; @@ -71,6 +73,13 @@ typedef enum { UBX_ID_MON_VER = 0x04, } ubx_class_mon_id; + +typedef enum { + UBX_ID_CFG_NAV5 = 0x24, + UBX_ID_CFG_RATE = 0x08, + UBX_ID_CFG_MSG = 0x01, +} ubx_class_cfg_id; + typedef enum { UBX_ID_ACK_ACK = 0x01, UBX_ID_ACK_NAK = 0x00, @@ -281,7 +290,7 @@ struct UBX_ACK_NAK { }; // MON message Class -#define UBX_MON_MAX_EXT 1 +#define UBX_MON_MAX_EXT 5 struct UBX_MON_VER { char swVersion[30]; char hwVersion[10]; @@ -343,8 +352,66 @@ struct UBXPacket { UBXPayload payload; }; +// Sent messages for configuration support + +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; +} ubx_cfg_nav5_t; + +typedef struct { + uint16_t measRate; + uint16_t navRate; + uint16_t timeRef; +} ubx_cfg_rate_t; + +typedef struct { + uint8_t msgClass; + uint8_t msgID; + uint8_t rate; +} ubx_cfg_msg_t; + +typedef struct { + uint8_t prolog[2]; + uint8_t class; + uint8_t id; + uint16_t len; +} UBXSentHeader_t; + +typedef union { + uint8_t buffer[0]; + struct { + UBXSentHeader_t header; + union { + ubx_cfg_nav5_t cfg_nav5; + ubx_cfg_rate_t cfg_rate; + ubx_cfg_msg_t cfg_msg; + } payload; + uint8_t resvd[2]; // added space for checksum bytes + } message; +} UBXSentPacket_t; + + bool checksum_ubx_message(struct UBXPacket *); uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *); + +void ubx_run_management_tasks(char * *buffer, uint16_t *count); int parse_ubx_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *); void load_mag_settings(); From af9dce3569d01673ba5bf3401f56d5240132594c Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 22 Aug 2014 02:43:47 +0200 Subject: [PATCH 21/74] OP-1370 - Settings definition --- shared/uavobjectdefinition/gpssettings.xml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/shared/uavobjectdefinition/gpssettings.xml b/shared/uavobjectdefinition/gpssettings.xml index dd4dd8cd5..789be2af4 100644 --- a/shared/uavobjectdefinition/gpssettings.xml +++ b/shared/uavobjectdefinition/gpssettings.xml @@ -4,6 +4,14 @@ + + + + + + + From 1846806d1785861b5ad0721332a7c16f7be4012d Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 22 Aug 2014 10:10:21 +0200 Subject: [PATCH 22/74] OP-1370 - Refactor AutoConfig code out of UBX.* --- flight/modules/GPS/GPS.c | 3 +- flight/modules/GPS/UBX.c | 86 +++------------------ flight/modules/GPS/inc/UBX.h | 60 +-------------- flight/modules/GPS/inc/ubx_autoconfig.h | 91 +++++++++++++++++++++++ flight/modules/GPS/ubx_autoconfig.c | 99 +++++++++++++++++++++++++ 5 files changed, 207 insertions(+), 132 deletions(-) create mode 100644 flight/modules/GPS/inc/ubx_autoconfig.h create mode 100644 flight/modules/GPS/ubx_autoconfig.c diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index d191ac8a0..9e0b7b622 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -48,6 +48,7 @@ #include "GPS.h" #include "NMEA.h" #include "UBX.h" +#include "inc/ubx_autoconfig.h" // **************** @@ -233,7 +234,7 @@ static void gpsTask(__attribute__((unused)) void *parameters) if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) { char *buffer = 0; uint16_t count = 0; - ubx_run_management_tasks(&buffer, &count); + ubx_autoconfig_run(&buffer, &count); // Something to send? if (count) { PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count); diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 0bc4fb26c..0761c31b8 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -47,8 +47,7 @@ static float mag_transform[3][3] = { static bool useMag = false; static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; -static struct UBX_ACK_ACK last_ack; -static struct UBX_ACK_NAK last_nak; + static bool usePvt = false; static uint32_t lastPvtTime = 0; @@ -101,19 +100,12 @@ const ubx_message_handler ubx_handler_table[] = { }; #define UBX_HANDLER_TABLE_SIZE NELEMENTS(ubx_handler_table) -// COnfiguration support -typedef enum { - INIT_STEP_ASK_VER = 0, - INIT_STEP_WAIT_VER = 1, - INIT_STEP_CONFIGURE = 2, - INIT_STEP_DONE = 3, -} initSteps; - -static initSteps currentConfigurationStep; -// timestamp of last operation -static uint32_t lastStepTimestampRaw = 0; // detected hw version -static int32_t hwVersion = -1; +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) @@ -452,24 +444,24 @@ static void parse_ubx_ack_ack(struct UBXPacket *ubx, __attribute__((unused)) GPS { struct UBX_ACK_ACK *ack_ack = &ubx->payload.ack_ack; - last_ack = *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; - last_nak = *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; - hwVersion = atoi(mon_ver->hwVersion); + ubxHwVersion = atoi(mon_ver->hwVersion); - sensorType = (hwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 : - ((hwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX); + sensorType = (ubxHwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 : + ((ubxHwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX); } @@ -569,61 +561,5 @@ void load_mag_settings() } } -UBXSentPacket_t working_packet; -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; -} -void 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); -} - -void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *count) -{ - prepare_packet(packet, classID, messageID, 0); - *count = packet->message.header.len + sizeof(UBXSentHeader_t) + 2; -} - -void ubx_run_management_tasks(char * *buffer, uint16_t *count) -{ - uint32_t elapsed = PIOS_DELAY_DiffuS(lastStepTimestampRaw); - - switch (currentConfigurationStep) { - case INIT_STEP_ASK_VER: - lastStepTimestampRaw = PIOS_DELAY_GetRaw(); - build_request(&working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, count); - *buffer = (char *)working_packet.buffer; - currentConfigurationStep = INIT_STEP_WAIT_VER; - break; - case INIT_STEP_WAIT_VER: - if (hwVersion > 0) { - currentConfigurationStep = INIT_STEP_CONFIGURE; - } else if (elapsed > UBX_REPLY_TIMEOUT) { - currentConfigurationStep = INIT_STEP_ASK_VER; - } - return; - - case INIT_STEP_CONFIGURE: - case INIT_STEP_DONE: - break; - } -} #endif // PIOS_INCLUDE_GPS_UBX_PARSER diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 973bc1bd6..2889d52cc 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -352,66 +352,14 @@ struct UBXPacket { UBXPayload payload; }; -// Sent messages for configuration support - -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; -} ubx_cfg_nav5_t; - -typedef struct { - uint16_t measRate; - uint16_t navRate; - uint16_t timeRef; -} ubx_cfg_rate_t; - -typedef struct { - uint8_t msgClass; - uint8_t msgID; - uint8_t rate; -} ubx_cfg_msg_t; - -typedef struct { - uint8_t prolog[2]; - uint8_t class; - uint8_t id; - uint16_t len; -} UBXSentHeader_t; - -typedef union { - uint8_t buffer[0]; - struct { - UBXSentHeader_t header; - union { - ubx_cfg_nav5_t cfg_nav5; - ubx_cfg_rate_t cfg_rate; - ubx_cfg_msg_t cfg_msg; - } payload; - uint8_t resvd[2]; // added space for checksum bytes - } message; -} UBXSentPacket_t; - +// 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 *); -void ubx_run_management_tasks(char * *buffer, uint16_t *count); int parse_ubx_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *); void load_mag_settings(); diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h new file mode 100644 index 000000000..272f1fdda --- /dev/null +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -0,0 +1,91 @@ +/** + ****************************************************************************** + * + * @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 +#include +#include "UBX.h" + +// Sent messages for configuration support + +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; +} ubx_cfg_nav5_t; + +typedef struct { + uint16_t measRate; + uint16_t navRate; + uint16_t timeRef; +} ubx_cfg_rate_t; + +typedef struct { + uint8_t msgClass; + uint8_t msgID; + uint8_t rate; +} ubx_cfg_msg_t; + +typedef struct { + uint8_t prolog[2]; + uint8_t class; + uint8_t id; + uint16_t len; +} UBXSentHeader_t; + +typedef union { + uint8_t buffer[0]; + struct { + UBXSentHeader_t header; + union { + ubx_cfg_nav5_t cfg_nav5; + ubx_cfg_rate_t cfg_rate; + ubx_cfg_msg_t cfg_msg; + } payload; + uint8_t resvd[2]; // added space for checksum bytes + } message; +} UBXSentPacket_t; + +void ubx_autoconfig_run(char * *buffer, uint16_t *count); + +#endif /* UBX_AUTOCONFIG_H_ */ diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c new file mode 100644 index 000000000..a68039fe6 --- /dev/null +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -0,0 +1,99 @@ +/** + ****************************************************************************** + * @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" + +typedef enum { + INIT_STEP_ASK_VER = 0, + INIT_STEP_WAIT_VER = 1, + INIT_STEP_CONFIGURE = 2, + INIT_STEP_DONE = 3, +} initSteps; + +static initSteps currentConfigurationStep; +// timestamp of last operation +static uint32_t lastStepTimestampRaw = 0; + +UBXSentPacket_t working_packet; +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; +} +void 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); +} + +void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *count) +{ + prepare_packet(packet, classID, messageID, 0); + *count = packet->message.header.len + sizeof(UBXSentHeader_t) + 2; +} + + +void ubx_autoconfig_run(char * *buffer, uint16_t *count) +{ + uint32_t elapsed = PIOS_DELAY_DiffuS(lastStepTimestampRaw); + + switch (currentConfigurationStep) { + case INIT_STEP_ASK_VER: + lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + build_request(&working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, count); + *buffer = (char *)working_packet.buffer; + currentConfigurationStep = INIT_STEP_WAIT_VER; + break; + case INIT_STEP_WAIT_VER: + if (ubxHwVersion > 0) { + currentConfigurationStep = INIT_STEP_CONFIGURE; + } else if (elapsed > UBX_REPLY_TIMEOUT) { + currentConfigurationStep = INIT_STEP_ASK_VER; + } + return; + + case INIT_STEP_CONFIGURE: + case INIT_STEP_DONE: + break; + } +} From f430af83c5a39c5716b184f604d26e948c6f28e1 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 22 Aug 2014 19:00:46 +0200 Subject: [PATCH 23/74] OP-1370 - Add configuration support for nav rate and dynamic model. Added stub for sentence rate configuration (enable_sentences(...)) --- flight/modules/GPS/GPS.c | 50 +++++-- flight/modules/GPS/inc/UBX.h | 3 + flight/modules/GPS/inc/ubx_autoconfig.h | 39 +++++- flight/modules/GPS/ubx_autoconfig.c | 169 ++++++++++++++++++++---- 4 files changed, 218 insertions(+), 43 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 9e0b7b622..b9e6ee2fe 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -55,13 +55,18 @@ // 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); +void updateGpsSettings(UAVObjEvent *ev); +#endif + // **************** // Private constants @@ -100,9 +105,7 @@ static uint32_t timeOfLastUpdateMs; #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) static struct GPS_RX_STATS gpsRxStats; #endif -#ifdef PIOS_INCLUDE_GPS_UBX_PARSER -void AuxMagSettingsUpdatedCb(UAVObjEvent *ev); -#endif + // **************** /** * Initialise the gps module @@ -166,7 +169,7 @@ int32_t GPSInitialize(void) AuxMagSettingsUpdatedCb(NULL); AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb); #endif - updateSettings(); + updateHwSettings(); #else if (gpsPort && gpsEnabled) { GPSPositionSensorInitialize(); @@ -178,7 +181,7 @@ int32_t GPSInitialize(void) #if defined(PIOS_GPS_SETS_HOMELOCATION) HomeLocationInitialize(); #endif - updateSettings(); + updateHwSettings(); } #endif /* if defined(REVOLUTION) */ @@ -196,7 +199,7 @@ int32_t GPSInitialize(void) gps_rx_buffer = NULL; } PIOS_Assert(gps_rx_buffer); - + GPSSettingsConnectCallback(updateGpsSettings); return 0; } @@ -227,6 +230,9 @@ static void gpsTask(__attribute__((unused)) void *parameters) timeOfLastCommandMs = timeNowMs; GPSPositionSensorGet(&gpspositionsensor); +#ifdef PIOS_INCLUDE_GPS_UBX_PARSER + updateGpsSettings(0); +#endif // Loop forever while (1) { uint8_t c; @@ -368,7 +374,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 @@ -404,12 +410,38 @@ static void updateSettings() } } } + #ifdef PIOS_INCLUDE_GPS_UBX_PARSER void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { load_mag_settings(); } -#endif + +void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) +{ + uint8_t ubxAutoConfig; + uint8_t ubxDynamicModel; + + ubx_autoconfig_settings_t newconfig; + + GPSSettingsUBXAutoConfigGet(&ubxAutoConfig); + GPSSettingsUBXRateGet(&newconfig.navRate); + GPSSettingsUBXDynamicModelGet(&ubxDynamicModel); + + newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_TRUE ? true : false; + 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; + ubx_autoconfig_set(newconfig); +} + +#endif /* ifdef PIOS_INCLUDE_GPS_UBX_PARSER */ /** * @} * @} diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 2889d52cc..fd9e3a77d 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -36,6 +36,9 @@ #include "auxmagsensor.h" #include "GPS.h" +#define UBX_HW_VERSION_8 80000 +#define UBX_HW_VERSION_7 70000 + #define UBX_REPLY_TIMEOUT (500 * 1000) #define UBX_SYNC1 0xb5 // UBX protocol synchronization characters diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index 272f1fdda..379979916 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -30,6 +30,32 @@ #include #include #include "UBX.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 + +// types + +// 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 struct { + bool autoconfigEnabled; + int8_t navRate; + ubx_config_dynamicmodel_t dynamicModel; +} ubx_autoconfig_settings_t; // Sent messages for configuration support @@ -52,26 +78,26 @@ typedef struct { uint16_t reserved2; uint32_t reserved3; uint32_t reserved4; -} ubx_cfg_nav5_t; +} __attribute__((packed)) ubx_cfg_nav5_t; typedef struct { uint16_t measRate; uint16_t navRate; uint16_t timeRef; -} ubx_cfg_rate_t; +} __attribute__((packed)) ubx_cfg_rate_t; typedef struct { uint8_t msgClass; uint8_t msgID; uint8_t rate; -} ubx_cfg_msg_t; +} __attribute__((packed)) ubx_cfg_msg_t; typedef struct { uint8_t prolog[2]; uint8_t class; uint8_t id; uint16_t len; -} UBXSentHeader_t; +} __attribute__((packed)) UBXSentHeader_t; typedef union { uint8_t buffer[0]; @@ -84,8 +110,9 @@ typedef union { } payload; uint8_t resvd[2]; // added space for checksum bytes } message; -} UBXSentPacket_t; +} __attribute__((packed)) UBXSentPacket_t; -void ubx_autoconfig_run(char * *buffer, uint16_t *count); +void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send); +void ubx_autoconfig_set(ubx_autoconfig_settings_t config); #endif /* UBX_AUTOCONFIG_H_ */ diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index a68039fe6..b45f8eccb 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -28,20 +28,39 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "inc/ubx_autoconfig.h" - +#include +// private type definitions typedef enum { - INIT_STEP_ASK_VER = 0, - INIT_STEP_WAIT_VER = 1, - INIT_STEP_CONFIGURE = 2, - INIT_STEP_DONE = 3, -} initSteps; + INIT_STEP_DISABLED = 0, + INIT_STEP_START, + INIT_STEP_ASK_VER, + INIT_STEP_WAIT_VER, + INIT_STEP_CONFIGURE, + INIT_STEP_ENABLE_SENTENCES, + INIT_STEP_DONE, +} initSteps_t; -static initSteps currentConfigurationStep; -// timestamp of last operation -static uint32_t lastStepTimestampRaw = 0; +typedef struct { + initSteps_t currentConfigurationStep; // 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 +} ubx_autoconfig_status_t; -UBXSentPacket_t working_packet; -void append_checksum(UBXSentPacket_t *packet) + +// private defines +#define LAST_CONFIG_SENT_START (-1) +#define LAST_CONFIG_SENT_COMPLETED (-2) + +// private variables + +// enable the autoconfiguration system +static bool enabled; + +static ubx_autoconfig_status_t *status = 0; + +static void append_checksum(UBXSentPacket_t *packet) { uint8_t i; uint8_t ck_a = 0; @@ -56,7 +75,11 @@ void append_checksum(UBXSentPacket_t *packet) packet->buffer[len] = ck_a; packet->buffer[len + 1] = ck_b; } -void prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t len) +/** + * 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; @@ -64,36 +87,126 @@ void prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, 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 } -void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *count) +static void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *bytes_to_send) { - prepare_packet(packet, classID, messageID, 0); - *count = packet->message.header.len + sizeof(UBXSentHeader_t) + 2; + *bytes_to_send = prepare_packet(packet, classID, messageID, 0); } - -void ubx_autoconfig_run(char * *buffer, uint16_t *count) +void config_rate(uint16_t *bytes_to_send) { - uint32_t elapsed = PIOS_DELAY_DiffuS(lastStepTimestampRaw); + 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; - switch (currentConfigurationStep) { + 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)); +} + +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)); +} + +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); + status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; + return; + + default: + status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; + return; + } + status->lastConfigSent++; +} + +static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) {} +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->currentConfigurationStep) { + case INIT_STEP_DISABLED: + case INIT_STEP_DONE: + return; + + case INIT_STEP_START: case INIT_STEP_ASK_VER: - lastStepTimestampRaw = PIOS_DELAY_GetRaw(); - build_request(&working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, count); - *buffer = (char *)working_packet.buffer; - currentConfigurationStep = INIT_STEP_WAIT_VER; + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send); + status->currentConfigurationStep = INIT_STEP_WAIT_VER; break; case INIT_STEP_WAIT_VER: + if (ubxHwVersion > 0) { - currentConfigurationStep = INIT_STEP_CONFIGURE; - } else if (elapsed > UBX_REPLY_TIMEOUT) { - currentConfigurationStep = INIT_STEP_ASK_VER; + status->lastConfigSent = LAST_CONFIG_SENT_START; + status->currentConfigurationStep = INIT_STEP_CONFIGURE; + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + return; + } + + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) { + status->currentConfigurationStep = INIT_STEP_ASK_VER; } return; case INIT_STEP_CONFIGURE: - case INIT_STEP_DONE: - break; + configure(bytes_to_send); + if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { + status->currentConfigurationStep = INIT_STEP_ENABLE_SENTENCES; + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } + return; + + case INIT_STEP_ENABLE_SENTENCES: + enable_sentences(bytes_to_send); + if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { + status->currentConfigurationStep = INIT_STEP_DONE; + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } + return; + } +} + +void ubx_autoconfig_set(ubx_autoconfig_settings_t config) +{ + enabled = false; + if (config.autoconfigEnabled) { + if (!status) { + status = (ubx_autoconfig_status_t *)pios_malloc(sizeof(ubx_autoconfig_status_t)); + memset(status, 0, sizeof(ubx_autoconfig_status_t)); + status->currentConfigurationStep = INIT_STEP_DISABLED; + } + status->currentSettings = config; + status->currentConfigurationStep = INIT_STEP_START; + enabled = true; } } From 69b76b665cdf326647609d109d1f3cfa7ff630b5 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 22 Aug 2014 21:27:50 +0200 Subject: [PATCH 24/74] OP-1370 - Add automatic sentence rate configuration based on HW version (llh+velned etc. vs PVT) --- flight/modules/GPS/inc/UBX.h | 2 - flight/modules/GPS/inc/ubx_autoconfig.h | 4 +- flight/modules/GPS/ubx_autoconfig.c | 127 +++++++++++++++++++----- 3 files changed, 104 insertions(+), 29 deletions(-) diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index fd9e3a77d..13c806dbc 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -39,8 +39,6 @@ #define UBX_HW_VERSION_8 80000 #define UBX_HW_VERSION_7 70000 -#define UBX_REPLY_TIMEOUT (500 * 1000) - #define UBX_SYNC1 0xb5 // UBX protocol synchronization characters #define UBX_SYNC2 0x62 diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index 379979916..d6f66c887 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -36,10 +36,10 @@ #define UBX_MAX_RATE_VER7 10 #define UBX_MAX_RATE 5 -// types +#define UBX_REPLY_TIMEOUT (500 * 1000) +#define UBX_MAX_RETRIES 5 // Enumeration options for field UBXDynamicModel - typedef enum { UBX_DYNMODEL_PORTABLE = 0, UBX_DYNMODEL_STATIONARY = 2, diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index b45f8eccb..0313a106e 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -36,19 +36,43 @@ typedef enum { INIT_STEP_ASK_VER, INIT_STEP_WAIT_VER, INIT_STEP_CONFIGURE, + INIT_STEP_CONFIGURE_WAIT_ACK, INIT_STEP_ENABLE_SENTENCES, + INIT_STEP_ENABLE_SENTENCES_WAIT_ACK, INIT_STEP_DONE, + INIT_STEP_ERROR, } initSteps_t; typedef struct { - initSteps_t currentConfigurationStep; // Current configuration "fsm" status - uint32_t lastStepTimestampRaw; // timestamp of last operation - UBXSentPacket_t working_packet; // outbound "buffer" + 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; } ubx_autoconfig_status_t; +ubx_cfg_msg_t msg_config_ubx6[] = { + { .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[] = { + { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_PVT, .rate = 1 }, + { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .rate = 0 }, + { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .rate = 1 }, + { .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_SVINFO, .rate = 10 }, +}; // private defines #define LAST_CONFIG_SENT_START (-1) #define LAST_CONFIG_SENT_COMPLETED (-2) @@ -113,6 +137,8 @@ void config_rate(uint16_t *bytes_to_send) 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) @@ -125,6 +151,8 @@ void config_nav(uint16_t *bytes_to_send) 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; } static void configure(uint16_t *bytes_to_send) @@ -142,10 +170,26 @@ static void configure(uint16_t *bytes_to_send) status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; return; } - status->lastConfigSent++; } -static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) {} +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; @@ -153,47 +197,80 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) if (!status || !enabled) { return; // autoconfig not enabled } - switch (status->currentConfigurationStep) { + 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(); + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send); - status->currentConfigurationStep = INIT_STEP_WAIT_VER; + status->currentStep = INIT_STEP_WAIT_VER; break; - case INIT_STEP_WAIT_VER: + case INIT_STEP_WAIT_VER: if (ubxHwVersion > 0) { status->lastConfigSent = LAST_CONFIG_SENT_START; - status->currentConfigurationStep = INIT_STEP_CONFIGURE; - status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + status->currentStep = INIT_STEP_CONFIGURE; + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); return; } if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) { - status->currentConfigurationStep = INIT_STEP_ASK_VER; - } - return; - - case INIT_STEP_CONFIGURE: - configure(bytes_to_send); - if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { - status->currentConfigurationStep = INIT_STEP_ENABLE_SENTENCES; - status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + status->currentStep = INIT_STEP_ASK_VER; } return; case INIT_STEP_ENABLE_SENTENCES: - enable_sentences(bytes_to_send); + 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->currentConfigurationStep = INIT_STEP_DONE; - status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + status->lastConfigSent = LAST_CONFIG_SENT_START; + status->currentStep = step_configure ? INIT_STEP_ENABLE_SENTENCES : INIT_STEP_DONE; + } 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) @@ -203,10 +280,10 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t config) if (!status) { status = (ubx_autoconfig_status_t *)pios_malloc(sizeof(ubx_autoconfig_status_t)); memset(status, 0, sizeof(ubx_autoconfig_status_t)); - status->currentConfigurationStep = INIT_STEP_DISABLED; + status->currentStep = INIT_STEP_DISABLED; } status->currentSettings = config; - status->currentConfigurationStep = INIT_STEP_START; + status->currentStep = INIT_STEP_START; enabled = true; } } From c4d5234ee655cce869aba63786405411e6f2e108 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 22 Aug 2014 22:32:57 +0200 Subject: [PATCH 25/74] OP-1370 - Show AutoConfig status in GPSPositionSensor --- flight/modules/GPS/GPS.c | 11 ++++++-- flight/modules/GPS/inc/ubx_autoconfig.h | 9 ++++++- flight/modules/GPS/ubx_autoconfig.c | 26 ++++++++++++++++--- .../uavobjectdefinition/gpspositionsensor.xml | 1 + 4 files changed, 40 insertions(+), 7 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index b9e6ee2fe..4717c75bf 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -257,8 +257,15 @@ static void gpsTask(__attribute__((unused)) void *parameters) break; #endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) - case GPSSETTINGS_DATAPROTOCOL_UBX: - res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); + case GPSSETTINGS_DATAPROTOCOL_UBX:{ + 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; + res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); + } break; #endif default: diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index d6f66c887..d7befed0a 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -39,6 +39,13 @@ #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, @@ -114,5 +121,5 @@ typedef union { void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send); void ubx_autoconfig_set(ubx_autoconfig_settings_t config); - +int32_t ubx_autoconfig_get_status(); #endif /* UBX_AUTOCONFIG_H_ */ diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 0313a106e..19bf179e3 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -51,7 +51,7 @@ typedef struct { 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; -} ubx_autoconfig_status_t; +} status_t; ubx_cfg_msg_t msg_config_ubx6[] = { { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .rate = 1 }, @@ -82,7 +82,7 @@ ubx_cfg_msg_t msg_config_ubx7[] = { // enable the autoconfiguration system static bool enabled; -static ubx_autoconfig_status_t *status = 0; +static status_t *status = 0; static void append_checksum(UBXSentPacket_t *packet) { @@ -278,8 +278,8 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t config) enabled = false; if (config.autoconfigEnabled) { if (!status) { - status = (ubx_autoconfig_status_t *)pios_malloc(sizeof(ubx_autoconfig_status_t)); - memset(status, 0, sizeof(ubx_autoconfig_status_t)); + status = (status_t *)pios_malloc(sizeof(status_t)); + memset(status, 0, sizeof(status_t)); status->currentStep = INIT_STEP_DISABLED; } status->currentSettings = config; @@ -287,3 +287,21 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t config) enabled = true; } } + +int32_t ubx_autoconfig_get_status(){ + if(!status){ + return UBX_AUTOCONFIG_STATUS_DISABLED; + } + switch (status->currentStep) { + case INIT_STEP_ERROR: + return UBX_AUTOCONFIG_STATUS_ERROR; + case INIT_STEP_DISABLED: + return UBX_AUTOCONFIG_STATUS_DISABLED; + case INIT_STEP_DONE: + return UBX_AUTOCONFIG_STATUS_DONE; + default: + break; + } + return UBX_AUTOCONFIG_STATUS_RUNNING; +} + diff --git a/shared/uavobjectdefinition/gpspositionsensor.xml b/shared/uavobjectdefinition/gpspositionsensor.xml index ced2c027a..a6a5ea194 100644 --- a/shared/uavobjectdefinition/gpspositionsensor.xml +++ b/shared/uavobjectdefinition/gpspositionsensor.xml @@ -13,6 +13,7 @@ + From 5bf866c96ec13ca2f6a484363a621bda58b0d8c8 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 22 Aug 2014 22:34:52 +0200 Subject: [PATCH 26/74] OP-1370 - Remove autoconfig from CC (removing sat info to spare some ram and adding autoconfig does not make much sense), fixed compilation for all other targets --- flight/modules/GPS/GPS.c | 38 +++++++++++++++---------- flight/modules/GPS/inc/ubx_autoconfig.h | 5 ++-- 2 files changed, 26 insertions(+), 17 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 4717c75bf..6594c2c43 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -48,8 +48,9 @@ #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 @@ -64,8 +65,10 @@ static float GravityAccel(float latitude, float longitude, float altitude); #ifdef PIOS_INCLUDE_GPS_UBX_PARSER void AuxMagSettingsUpdatedCb(UAVObjEvent *ev); +#ifndef PIOS_GPS_MINIMAL void updateGpsSettings(UAVObjEvent *ev); #endif +#endif // **************** // Private constants @@ -199,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; } @@ -230,13 +235,13 @@ static void gpsTask(__attribute__((unused)) void *parameters) timeOfLastCommandMs = timeNowMs; GPSPositionSensorGet(&gpspositionsensor); -#ifdef PIOS_INCLUDE_GPS_UBX_PARSER +#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) +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) { char *buffer = 0; uint16_t count = 0; @@ -257,16 +262,19 @@ static void gpsTask(__attribute__((unused)) void *parameters) break; #endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) - case GPSSETTINGS_DATAPROTOCOL_UBX:{ - 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; - res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); - } - break; + 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; #endif default: res = NO_PARSER; // this should not happen @@ -423,7 +431,7 @@ 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; @@ -447,7 +455,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) UBX_DYNMODEL_AIRBORNE1G; ubx_autoconfig_set(newconfig); } - +#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */ #endif /* ifdef PIOS_INCLUDE_GPS_UBX_PARSER */ /** * @} diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index d7befed0a..31fd51568 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -27,9 +27,10 @@ #ifndef UBX_AUTOCONFIG_H_ #define UBX_AUTOCONFIG_H_ +#include "UBX.h" #include #include -#include "UBX.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 @@ -37,7 +38,7 @@ #define UBX_MAX_RATE 5 #define UBX_REPLY_TIMEOUT (500 * 1000) -#define UBX_MAX_RETRIES 5 +#define UBX_MAX_RETRIES 5 // types typedef enum { From 5a3ea5fde03af0ac1305648305ee60538dbdc607 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 22 Aug 2014 23:16:38 +0200 Subject: [PATCH 27/74] OP-1370 - Add an option to store the configuration onto GPS --- flight/modules/GPS/GPS.c | 3 +- flight/modules/GPS/inc/UBX.h | 9 ++-- flight/modules/GPS/inc/ubx_autoconfig.h | 9 +++- flight/modules/GPS/ubx_autoconfig.c | 50 +++++++++++++++------- shared/uavobjectdefinition/gpssettings.xml | 5 ++- 5 files changed, 53 insertions(+), 23 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 6594c2c43..24f97ba65 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -443,7 +443,8 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) GPSSettingsUBXRateGet(&newconfig.navRate); GPSSettingsUBXDynamicModelGet(&ubxDynamicModel); - newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_TRUE ? true : false; + newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_FALSE ? false : true; + newconfig.storeSettings = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_STORE; newconfig.dynamicModel = ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE : ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY : ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN : diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 13c806dbc..6cc285795 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -36,11 +36,11 @@ #include "auxmagsensor.h" #include "GPS.h" -#define UBX_HW_VERSION_8 80000 -#define UBX_HW_VERSION_7 70000 +#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 @@ -79,6 +79,7 @@ typedef enum { UBX_ID_CFG_NAV5 = 0x24, UBX_ID_CFG_RATE = 0x08, UBX_ID_CFG_MSG = 0x01, + UBX_ID_CFG_CFG = 0x09, } ubx_class_cfg_id; typedef enum { diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index 31fd51568..d2e00754c 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -61,11 +61,17 @@ typedef enum { typedef struct { bool autoconfigEnabled; + bool storeSettings; int8_t navRate; ubx_config_dynamicmodel_t dynamicModel; } ubx_autoconfig_settings_t; // Sent messages for configuration support +typedef struct { + uint32_t clearMask; + uint32_t saveMask; + uint32_t loadMask; +} __attribute__((packed)) ubx_cfg_cfg_t; typedef struct { uint16_t mask; @@ -112,9 +118,10 @@ typedef union { 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_msg_t cfg_msg; } payload; uint8_t resvd[2]; // added space for checksum bytes } message; diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 19bf179e3..215d2ab50 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -35,10 +35,10 @@ typedef enum { INIT_STEP_START, INIT_STEP_ASK_VER, INIT_STEP_WAIT_VER, - INIT_STEP_CONFIGURE, - INIT_STEP_CONFIGURE_WAIT_ACK, 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; @@ -155,6 +155,15 @@ void config_nav(uint16_t *bytes_to_send) status->requiredAck.msgID = UBX_ID_CFG_NAV5; } +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 + *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) { @@ -163,6 +172,13 @@ static void configure(uint16_t *bytes_to_send) break; case LAST_CONFIG_SENT_START + 1: config_nav(bytes_to_send); + if (!status->currentSettings.storeSettings) { + // skips saving + status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; + } + break; + case LAST_CONFIG_SENT_START + 2: + config_save(bytes_to_send); status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; return; @@ -190,6 +206,7 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; } } + void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) { *bytes_to_send = 0; @@ -213,7 +230,7 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) case INIT_STEP_WAIT_VER: if (ubxHwVersion > 0) { status->lastConfigSent = LAST_CONFIG_SENT_START; - status->currentStep = INIT_STEP_CONFIGURE; + status->currentStep = INIT_STEP_ENABLE_SENTENCES; status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); return; } @@ -236,7 +253,7 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *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_ENABLE_SENTENCES : INIT_STEP_DONE; + 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; } @@ -288,20 +305,23 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t config) } } -int32_t ubx_autoconfig_get_status(){ - if(!status){ +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; + case INIT_STEP_ERROR: + return UBX_AUTOCONFIG_STATUS_ERROR; + + case INIT_STEP_DISABLED: + return UBX_AUTOCONFIG_STATUS_DISABLED; + + case INIT_STEP_DONE: + return UBX_AUTOCONFIG_STATUS_DONE; + + default: + break; } return UBX_AUTOCONFIG_STATUS_RUNNING; } - diff --git a/shared/uavobjectdefinition/gpssettings.xml b/shared/uavobjectdefinition/gpssettings.xml index 789be2af4..f16773551 100644 --- a/shared/uavobjectdefinition/gpssettings.xml +++ b/shared/uavobjectdefinition/gpssettings.xml @@ -4,8 +4,9 @@ - - + + From 1d3f9399c3ffbe4c1d61deceb8d7ffb8d7e586da Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 22 Aug 2014 23:35:13 +0200 Subject: [PATCH 28/74] OP-1370 - Fix sensortype not shown for non PVT GPSs --- flight/modules/GPS/UBX.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 0761c31b8..6265262eb 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -380,7 +380,6 @@ static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsP GPSTimeSet(&GpsTime); } #endif - GpsPosition->SensorType = sensorType; } #if !defined(PIOS_GPS_MINIMAL) @@ -528,6 +527,8 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi } } + GpsPosition->SensorType = sensorType; + if (msgtracker.msg_received == ALL_RECEIVED) { GPSPositionSensorSet(GpsPosition); msgtracker.msg_received = NONE_RECEIVED; From f81effa7eafd2007cb81cb81583da5ce7a5312dd Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Sat, 23 Aug 2014 15:45:16 +0200 Subject: [PATCH 29/74] OP-1455 Send the current slider values when enabling Test outputs --- .../src/plugins/config/configoutputwidget.cpp | 16 ++++++++++++++++ .../src/plugins/config/configoutputwidget.h | 4 ++++ 2 files changed, 20 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp index c06d28e5b..3cd43c4a3 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp @@ -126,6 +126,17 @@ void ConfigOutputWidget::enableControls(bool enable) ui->channelOutTest->setEnabled(enable); } +/** + Force update all channels with the values in the OutputChannelForms. + */ +void ConfigOutputWidget::sendAllChannelTests() +{ + for (unsigned int i = 0; i < ActuatorCommand::CHANNEL_NUMELEM; i++) { + OutputChannelForm *form = getOutputChannelForm(i); + sendChannelTest(i, form->neutral()); + } +} + /** Toggles the channel testing mode by making the GCS take over the ActuatorCommand objects @@ -178,6 +189,11 @@ void ConfigOutputWidget::runChannelTests(bool state) } obj->setMetadata(mdata); obj->updated(); + + // Setup the correct initial channel values when the channel testing mode is turned on. + if (state) { + sendAllChannelTests(); + } } OutputChannelForm *ConfigOutputWidget::getOutputChannelForm(const int index) const diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.h b/ground/openpilotgcs/src/plugins/config/configoutputwidget.h index fda2922c6..3105b25d9 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.h @@ -58,7 +58,11 @@ private: void updateChannelInSlider(QSlider *slider, QLabel *min, QLabel *max, QCheckBox *rev, int value); void assignOutputChannel(UAVDataObject *obj, QString &str); + OutputChannelForm *getOutputChannelForm(const int index) const; + + void sendAllChannelTests(); + int mccDataRate; UAVObject::Metadata accInitialData; From 8418804ce48dbb69ef68d5794be59962e6bde211 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 25 Aug 2014 16:00:33 +0200 Subject: [PATCH 30/74] OP-1406 - Revert magstate to non normalized values --- flight/modules/StateEstimation/filtermag.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index a4d9c24bb..aded510ef 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -143,9 +143,6 @@ static filterResult filter(stateFilter *self, stateEstimation *state) } if (temp_status != MAGSTATUS_INVALID) { - temp_mag[0] *= this->invMagBe; - temp_mag[1] *= this->invMagBe; - temp_mag[2] *= this->invMagBe; state->mag[0] = temp_mag[0]; state->mag[1] = temp_mag[1]; state->mag[2] = temp_mag[2]; From 52420c2ed6b42df692ac7c5b441875af92f16a43 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Mon, 25 Aug 2014 19:15:03 +0200 Subject: [PATCH 31/74] OP-1459 Turn off useOpenGL by default in OPMapGadget --- .../openpilotgcs/default_configurations/OpenPilotGCS.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml index 88032dc41..0f848b08f 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml @@ -1779,7 +1779,7 @@ false mapquad.png true - true + false @@ -1799,7 +1799,7 @@ false airplanepip.png true - true + false @@ -1819,7 +1819,7 @@ false mapquad.png true - true + false From 398a546cacbfc0f58f21e63d65583547e09a733c Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 28 Aug 2014 11:14:43 +0200 Subject: [PATCH 32/74] Significantly lower some EKF variance settings to improve stability and tracking accuracy. --- shared/uavobjectdefinition/ekfconfiguration.xml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/shared/uavobjectdefinition/ekfconfiguration.xml b/shared/uavobjectdefinition/ekfconfiguration.xml index a44a8692c..fee4f68e7 100644 --- a/shared/uavobjectdefinition/ekfconfiguration.xml +++ b/shared/uavobjectdefinition/ekfconfiguration.xml @@ -24,8 +24,8 @@ + 0.01, 0.01, 0.01, + 0.000001, 0.000001, 0.000001"> GyroX GyroY @@ -39,10 +39,10 @@ + 1, 1, 1000000, + 0.001, 0.001, 0.001, + 10, 10, 10, + 0.01"> GPSPosNorth GPSPosEast From e5d1676d44d557b6f7a03cd2f2ae58748a6f5312 Mon Sep 17 00:00:00 2001 From: James Duley Date: Fri, 29 Aug 2014 09:01:30 +1200 Subject: [PATCH 33/74] OP-1463 allow for alternative_OPENPILOT_TOOLS_DIR:added check to see if it is set and use it if possible --- ground/openpilotgcs/openpilotgcs.pri | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/openpilotgcs.pri b/ground/openpilotgcs/openpilotgcs.pri index 66315baf6..577e44db9 100644 --- a/ground/openpilotgcs/openpilotgcs.pri +++ b/ground/openpilotgcs/openpilotgcs.pri @@ -97,7 +97,8 @@ macx { GCS_QT_PLUGINS_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/plugins GCS_QT_QML_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/qml - TOOLS_DIR = $$clean_path($$GCS_SOURCE_TREE/../../tools) + TOOLS_DIR = $$(OPENPILOT_TOOLS_DIR) + isEmpty(TOOLS_DIR):TOOLS_DIR = $$clean_path($$GCS_SOURCE_TREE/../../tools) QT_INSTALL_DIR = $$clean_path($$[QT_INSTALL_LIBS]/../../../..) equals(QT_INSTALL_DIR, $$TOOLS_DIR) { copyqt = 1 From 5f346d81e907313d4d95209a99c3afd88e514cd4 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Fri, 29 Aug 2014 19:45:29 +0200 Subject: [PATCH 34/74] OP-1349_GCS Neutral_Spinboxes_in_Input_and_Output Disable keyboard tracking on inputform spinboxes (min, max, neutral). --- .../openpilotgcs/src/plugins/config/inputchannelform.ui | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui index 2f4db720c..0a3fc987e 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui @@ -120,6 +120,9 @@ margin:1px; QAbstractSpinBox::UpDownArrows + + false + 9999 @@ -670,6 +673,9 @@ margin:1px; QAbstractSpinBox::UpDownArrows + + false + 9999 @@ -762,6 +768,9 @@ margin:1px; Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + false + 9999 From c5d57a763efb4c965a14694cfdf63eefba8d7d2b Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 29 Aug 2014 21:13:13 +0200 Subject: [PATCH 35/74] OP-1465 - Update stm32flash repository to https://code.google.com/p/stm32flash/ and fetch a newer version (a358bd1f025d) --- make/tools.mk | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/make/tools.mk b/make/tools.mk index d66dd388d..28afd4732 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -1095,13 +1095,18 @@ openocd_clean: STM32FLASH_DIR := $(TOOLS_DIR)/stm32flash .PHONY: stm32flash_install -stm32flash_install: STM32FLASH_URL := http://stm32flash.googlecode.com/svn/trunk -stm32flash_install: STM32FLASH_REV := 61 +stm32flash_install: STM32FLASH_URL := https://code.google.com/p/stm32flash/ +stm32flash_install: STM32FLASH_REV := a358bd1f025d stm32flash_install: stm32flash_clean # download the source $(V0) @echo " DOWNLOAD $(STM32FLASH_URL) @ r$(STM32FLASH_REV)" - $(V1) svn export -q -r "$(STM32FLASH_REV)" "$(STM32FLASH_URL)" "$(STM32FLASH_DIR)" - + $(V1) [ ! -d "$(STM32FLASH_DIR)" ] || $(RM) -rf "$(STM32FLASH_DIR)" + $(V1) mkdir -p "$(STM32FLASH_DIR)" + $(V1) git clone --no-checkout $(STM32FLASH_URL) "$(STM32FLASH_DIR)" + $(V1) ( \ + cd $(STM32FLASH_DIR) ; \ + git checkout -q $(STM32FLASH_REV) ; \ + ) # build $(V0) @echo " BUILD $(STM32FLASH_DIR)" $(V1) $(MAKE) --silent -C $(STM32FLASH_DIR) all From 4715219a4b75ef3fe83228e800fb8501b73c0a2f Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 31 Aug 2014 23:39:30 +0200 Subject: [PATCH 36/74] OP-1434 - Enable magFilter for CF as it is needed to get Mag data for initial estimation --- flight/modules/StateEstimation/stateestimation.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 3bce31c52..c3c105c53 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -162,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, + } } } } From ec07016aee74e5a9c64e28ea57f0bd784914115b Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 1 Sep 2014 14:51:43 +0200 Subject: [PATCH 37/74] OP-1465 - Fix build on windows, force the removal on cleanup --- make/tools.mk | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/make/tools.mk b/make/tools.mk index 28afd4732..afb1349dc 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -1093,7 +1093,9 @@ openocd_clean: $(V1) [ ! -d "$(OPENOCD_DIR)" ] || $(RM) -r "$(OPENOCD_DIR)" STM32FLASH_DIR := $(TOOLS_DIR)/stm32flash - +ifeq ($(UNAME), Windows) + STM32FLASH_BUILD_OPTIONS := "CC=GCC" +endif .PHONY: stm32flash_install stm32flash_install: STM32FLASH_URL := https://code.google.com/p/stm32flash/ stm32flash_install: STM32FLASH_REV := a358bd1f025d @@ -1109,12 +1111,12 @@ stm32flash_install: stm32flash_clean ) # build $(V0) @echo " BUILD $(STM32FLASH_DIR)" - $(V1) $(MAKE) --silent -C $(STM32FLASH_DIR) all + $(V1) $(MAKE) --silent -C $(STM32FLASH_DIR) all $(STM32FLASH_BUILD_OPTIONS) .PHONY: stm32flash_clean stm32flash_clean: $(V0) @echo " CLEAN $(STM32FLASH_DIR)" - $(V1) [ ! -d "$(STM32FLASH_DIR)" ] || $(RM) -r "$(STM32FLASH_DIR)" + $(V1) [ ! -d "$(STM32FLASH_DIR)" ] || $(RM) -rf "$(STM32FLASH_DIR)" DFUUTIL_DIR := $(TOOLS_DIR)/dfu-util From 3157c4e501b17de80ca3598ac027b9bb0591f013 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 2 Sep 2014 00:35:26 +0200 Subject: [PATCH 38/74] OP-1370 - Fix configuration storage on GPS --- flight/modules/GPS/inc/ubx_autoconfig.h | 4 ++++ flight/modules/GPS/ubx_autoconfig.c | 3 ++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index d2e00754c..3f5ffbeef 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -66,11 +66,15 @@ typedef struct { 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 { diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 215d2ab50..b8bef3efc 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -159,7 +159,8 @@ 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.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; From 78662eb3564164f8611ecc936e2c35c056edb0ae Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 2 Sep 2014 00:36:04 +0200 Subject: [PATCH 39/74] OP-1370 - Explicitly disable unneeded messages --- flight/modules/GPS/inc/UBX.h | 58 +++++++++++++++++++----- flight/modules/GPS/ubx_autoconfig.c | 69 ++++++++++++++++++++++------- 2 files changed, 102 insertions(+), 25 deletions(-) diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 6cc285795..1b1256e1f 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -51,18 +51,29 @@ typedef enum { 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 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_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 { @@ -71,15 +82,23 @@ typedef enum { } ubx_class_op_id; typedef enum { - UBX_ID_MON_VER = 0x04, + 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 { @@ -87,6 +106,25 @@ typedef enum { 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 diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index b8bef3efc..b21d915ec 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -54,25 +54,64 @@ typedef struct { } status_t; ubx_cfg_msg_t msg_config_ubx6[] = { - { .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 }, + // 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[] = { - { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_PVT, .rate = 1 }, - { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .rate = 0 }, - { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .rate = 1 }, - { .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_SVINFO, .rate = 10 }, + // 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) From 6243cad7682775c02bbd5b8672032b4db1433907 Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Tue, 2 Sep 2014 23:18:45 +1000 Subject: [PATCH 40/74] Change mouse pointr type to show this is a link --- ground/openpilotgcs/src/plugins/welcome/qml/main.qml | 9 +++++++-- ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp | 2 +- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/welcome/qml/main.qml b/ground/openpilotgcs/src/plugins/welcome/qml/main.qml index 7425d906d..97ddb8e5d 100644 --- a/ground/openpilotgcs/src/plugins/welcome/qml/main.qml +++ b/ground/openpilotgcs/src/plugins/welcome/qml/main.qml @@ -135,15 +135,20 @@ Rectangle { text: welcomePlugin.newVersionText anchors.rightMargin: 0 font.bold: true + font.underline: true styleColor: "#00000000" horizontalAlignment: Text.AlignHCenter font.pixelSize: 14 anchors.left: textOpVersion.right - MouseArea{ + id: mouseAreaOpVersionAvailable + hoverEnabled: true + cursorShape: Qt.PointingHandCursor width: parent.width height: parent.height - onClicked: welcomePlugin.openUrl("http://wiki.openpilot.org/display/BUILDS/OpenPilot+Software+Downloads") + onClicked: { + welcomePlugin.openUrl("http://wiki.openpilot.org/display/BUILDS/OpenPilot+Software+Downloads") + } } } } diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp b/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp index 090a77d85..f767fd21d 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp +++ b/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp @@ -169,7 +169,7 @@ void WelcomeMode::networkResponseReady(QNetworkReply* reply) if(version != VersionInfo::tagOrHash8()) { - m_newVersionText = tr("(Update Available: %1)").arg(version); + m_newVersionText = tr("Update Available: %1").arg(version); emit newVersionTextChanged(); } } From 650cbd3b5213d14bdad7a3be64f7fc9c777334b1 Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Wed, 3 Sep 2014 02:46:26 +1000 Subject: [PATCH 41/74] Uncrustify --- .../src/plugins/welcome/welcomemode.cpp | 21 +++++++------------ .../src/plugins/welcome/welcomemode.h | 7 +++---- 2 files changed, 11 insertions(+), 17 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp b/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp index f767fd21d..5fab6684e 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp +++ b/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp @@ -80,20 +80,17 @@ WelcomeMode::WelcomeMode() : m_d->quickView->setSource(QUrl("qrc:/welcome/qml/main.qml")); m_container = NULL; - QNetworkAccessManager* networkAccessManager = new QNetworkAccessManager; + QNetworkAccessManager *networkAccessManager = new QNetworkAccessManager; // Only attempt to request our version info if the network is accessible - if(networkAccessManager->networkAccessible() == QNetworkAccessManager::Accessible) - { - connect(networkAccessManager, SIGNAL(finished(QNetworkReply*)), this, SLOT(networkResponseReady(QNetworkReply*))); + if (networkAccessManager->networkAccessible() == QNetworkAccessManager::Accessible) { + connect(networkAccessManager, SIGNAL(finished(QNetworkReply *)), this, SLOT(networkResponseReady(QNetworkReply *))); // This will delete the network access manager instance when we're done - connect(networkAccessManager, SIGNAL(finished(QNetworkReply*)), networkAccessManager, SLOT(deleteLater())); + connect(networkAccessManager, SIGNAL(finished(QNetworkReply *)), networkAccessManager, SLOT(deleteLater())); networkAccessManager->get(QNetworkRequest(QUrl("http://www.openpilot.org/opver"))); - } - else - { + } else { // No network, can delete this now as we don't need it. delete networkAccessManager; } @@ -158,17 +155,15 @@ void WelcomeMode::triggerAction(const QString &actionId) Core::ModeManager::instance()->triggerAction(actionId); } -void WelcomeMode::networkResponseReady(QNetworkReply* reply) +void WelcomeMode::networkResponseReady(QNetworkReply *reply) { - if(reply != NULL) - { + if (reply != NULL) { QString version(reply->readAll()); version = version.trimmed(); reply->deleteLater(); - if(version != VersionInfo::tagOrHash8()) - { + if (version != VersionInfo::tagOrHash8()) { m_newVersionText = tr("Update Available: %1").arg(version); emit newVersionTextChanged(); } diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomemode.h b/ground/openpilotgcs/src/plugins/welcome/welcomemode.h index ab85912b9..a51cf3909 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomemode.h +++ b/ground/openpilotgcs/src/plugins/welcome/welcomemode.h @@ -45,8 +45,7 @@ namespace Welcome { struct WelcomeModePrivate; class WELCOME_EXPORT WelcomeMode : public Core::IMode { - Q_OBJECT - Q_PROPERTY(QString versionString READ versionString) + Q_OBJECT Q_PROPERTY(QString versionString READ versionString) Q_PROPERTY(QString newVersionText READ newVersionText NOTIFY newVersionTextChanged) public: @@ -92,8 +91,8 @@ private: int m_priority; QString m_newVersionText; - private slots: - void networkResponseReady(QNetworkReply* reply); +private slots: + void networkResponseReady(QNetworkReply *reply); }; } // namespace Welcome From 2ddbb72bcfeec7bc90e0493118ce0d63357c0bd8 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 2 Sep 2014 21:26:43 +0200 Subject: [PATCH 42/74] OP-1370 - Fixes parameters name, values and cosmetic issues. --- flight/modules/GPS/GPS.c | 6 ++-- flight/modules/GPS/ubx_autoconfig.c | 40 +++++++++++----------- flight/modules/StateEstimation/filterlla.c | 2 +- shared/uavobjectdefinition/gpssettings.xml | 15 ++++---- 4 files changed, 31 insertions(+), 32 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 24f97ba65..a88db3f7c 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -300,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); @@ -442,9 +442,9 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) GPSSettingsUBXAutoConfigGet(&ubxAutoConfig); GPSSettingsUBXRateGet(&newconfig.navRate); GPSSettingsUBXDynamicModelGet(&ubxDynamicModel); + newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true; + newconfig.storeSettings = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE; - newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_FALSE ? false : true; - newconfig.storeSettings = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_STORE; newconfig.dynamicModel = ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE : ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY : ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN : diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index b21d915ec..9e46427a0 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -55,30 +55,30 @@ typedef struct { 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_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_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 }, + { .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 }, + { .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[] = { diff --git a/flight/modules/StateEstimation/filterlla.c b/flight/modules/StateEstimation/filterlla.c index 327fa9a00..352e0000c 100644 --- a/flight/modules/StateEstimation/filterlla.c +++ b/flight/modules/StateEstimation/filterlla.c @@ -103,7 +103,7 @@ static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstim GPSPositionSensorGet(&gpsdata); // check if we have a valid GPS signal (not checked by StateEstimation istelf) - if ((gpsdata.PDOP < this->settings.MaxPDOP) && (gpsdata.Satellites >= this->settings.MinSattelites) && + if ((gpsdata.PDOP < this->settings.MaxPDOP) && (gpsdata.Satellites >= this->settings.MinSatellites) && (gpsdata.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && (gpsdata.Latitude != 0 || gpsdata.Longitude != 0)) { int32_t LLAi[3] = { diff --git a/shared/uavobjectdefinition/gpssettings.xml b/shared/uavobjectdefinition/gpssettings.xml index f16773551..81fade958 100644 --- a/shared/uavobjectdefinition/gpssettings.xml +++ b/shared/uavobjectdefinition/gpssettings.xml @@ -2,17 +2,16 @@ GPS Module specific settings - + - - - - - - + + + + - From 884f797d6142acf76ae03d91ed0f60fb3bee05a4 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 2 Sep 2014 21:26:56 +0200 Subject: [PATCH 43/74] OP-1370 - Add SBAS configuration options --- flight/modules/GPS/GPS.c | 55 ++++++++++++++++++++-- flight/modules/GPS/inc/ubx_autoconfig.h | 54 +++++++++++++++++++-- flight/modules/GPS/ubx_autoconfig.c | 34 ++++++++++++- shared/uavobjectdefinition/gpssettings.xml | 4 ++ 4 files changed, 139 insertions(+), 8 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index a88db3f7c..9729c5189 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -436,15 +436,17 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) { uint8_t ubxAutoConfig; uint8_t ubxDynamicModel; - + uint8_t ubxSbasMode; ubx_autoconfig_settings_t newconfig; + uint8_t ubxSbasSats; - GPSSettingsUBXAutoConfigGet(&ubxAutoConfig); - GPSSettingsUBXRateGet(&newconfig.navRate); - GPSSettingsUBXDynamicModelGet(&ubxDynamicModel); + 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 : @@ -454,6 +456,51 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) 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) */ diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index 3f5ffbeef..e01391a7d 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -59,10 +59,27 @@ typedef enum { 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; - int8_t navRate; + 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; @@ -110,6 +127,36 @@ typedef struct { 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; @@ -126,6 +173,7 @@ typedef union { 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; diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 9e46427a0..74aa89179 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -194,6 +194,35 @@ void config_nav(uint16_t *bytes_to_send) 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)); @@ -212,12 +241,15 @@ static void configure(uint16_t *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 + 2: + case LAST_CONFIG_SENT_START + 3: config_save(bytes_to_send); status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; return; diff --git a/shared/uavobjectdefinition/gpssettings.xml b/shared/uavobjectdefinition/gpssettings.xml index 81fade958..b8e5a37b1 100644 --- a/shared/uavobjectdefinition/gpssettings.xml +++ b/shared/uavobjectdefinition/gpssettings.xml @@ -12,6 +12,10 @@ + + + + From 157ab9c0d2be88bcd05fb2d36c31b180b4fa4ab4 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 3 Sep 2014 18:12:27 +0200 Subject: [PATCH 44/74] OP-1370 - Change default options to use SBAS only as ranging source --- shared/uavobjectdefinition/gpssettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/gpssettings.xml b/shared/uavobjectdefinition/gpssettings.xml index b8e5a37b1..e2919eec8 100644 --- a/shared/uavobjectdefinition/gpssettings.xml +++ b/shared/uavobjectdefinition/gpssettings.xml @@ -13,7 +13,7 @@ - + From 9bfd6488790ae376d7915cf5488ec923247437af Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Wed, 3 Sep 2014 21:19:15 +0200 Subject: [PATCH 45/74] OP-1077 Prevent GCS flight log corruption when no flight log is chosen --- ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp b/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp index 749038014..e6238b87f 100644 --- a/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp +++ b/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp @@ -76,8 +76,10 @@ QIODevice *LoggingConnection::openDevice(const QString &deviceName) QString fileName = QFileDialog::getOpenFileName(NULL, tr("Open file"), QString(""), tr("OpenPilot Log (*.opl)")); if (!fileName.isNull()) { startReplay(fileName); + return &logFile; } - return &logFile; + + return NULL; } void LoggingConnection::startReplay(QString file) From ec0d44e1b0060e713f944041760f6f403dc9c316 Mon Sep 17 00:00:00 2001 From: Karl Knutsson Date: Thu, 4 Sep 2014 18:15:36 +0200 Subject: [PATCH 46/74] OP-1473 Fix EOF handling for R7008SB --- flight/pios/common/pios_sbus.c | 2 +- flight/pios/inc/pios_sbus_priv.h | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c index 640ddb748..69089e9cd 100644 --- a/flight/pios/common/pios_sbus.c +++ b/flight/pios/common/pios_sbus.c @@ -250,7 +250,7 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b) state->received_data[state->byte_count - 1] = b; state->byte_count++; } else { - if (b == SBUS_EOF_BYTE || (b % SBUS_R7008SB_EOF_COUNTER_MASK) == SBUS_R7008SB_EOF_BYTE) { + if (b == SBUS_EOF_BYTE || (b & SBUS_R7008SB_EOF_COUNTER_MASK) == 0) { /* full frame received */ uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3]; if (flags & SBUS_FLAG_FL) { diff --git a/flight/pios/inc/pios_sbus_priv.h b/flight/pios/inc/pios_sbus_priv.h index 5f104e25f..791683831 100644 --- a/flight/pios/inc/pios_sbus_priv.h +++ b/flight/pios/inc/pios_sbus_priv.h @@ -66,8 +66,7 @@ #define SBUS_FLAG_FL 0x04 #define SBUS_FLAG_FS 0x08 -#define SBUS_R7008SB_EOF_COUNTER_MASK 0xCF -#define SBUS_R7008SB_EOF_BYTE 0x04 +#define SBUS_R7008SB_EOF_COUNTER_MASK 0xCB /* * S.Bus protocol provides 16 proportional and 2 discrete channels. From 4a19f98c95a2c9bdba85188752e6a1f237917d88 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 6 Sep 2014 13:51:30 +0200 Subject: [PATCH 47/74] OP-1156 hotfix in pathfollower code - use correct VerticalPosPI.Ki and ILimit + uavobject typo --- flight/modules/PathFollower/pathfollower.c | 2 +- shared/uavobjectdefinition/vtolpathfollowersettings.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 039e85722..bd6372b17 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -376,7 +376,7 @@ static uint8_t updateAutoPilotVtol() } // vertical positon control PID loops works the same in both regular and fallback modes, setup according to settings - pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.VerticalPosPI.Ki, 0.0f, vtolPathFollowerSettings.VerticalPosPI.ILimit); switch (followermode) { case FOLLOWER_REGULAR: diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index fec77cb55..051d125c0 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -6,7 +6,7 @@ - + From 9e13c9c8da600a132d99878a89f563f48beb3196 Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Sun, 7 Sep 2014 15:52:22 +1000 Subject: [PATCH 48/74] Atom tinkering, prepare for new config system --- .../uavobjectdefinition/stabilizationsettingsbank1.xml | 10 +++++----- .../uavobjectdefinition/stabilizationsettingsbank2.xml | 10 +++++----- .../uavobjectdefinition/stabilizationsettingsbank3.xml | 10 +++++----- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml index d65aab250..0b38ec4c6 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml @@ -2,15 +2,15 @@ Currently selected PID bank - - - - + + + + - + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml index de23ee155..b562f12d3 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml @@ -2,15 +2,15 @@ Currently selected PID bank - - - - + + + + - + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml index 4c8ff660d..2500c68ae 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml @@ -2,15 +2,15 @@ Currently selected PID bank - - - - + + + + - + From 7a98dc722707710a096224eaab7378a1e93d9549 Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Sun, 7 Sep 2014 16:49:08 +1000 Subject: [PATCH 49/74] More rate work --- shared/uavobjectdefinition/stabilizationsettingsbank1.xml | 2 +- shared/uavobjectdefinition/stabilizationsettingsbank2.xml | 2 +- shared/uavobjectdefinition/stabilizationsettingsbank3.xml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml index 0b38ec4c6..7c8a71e64 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml @@ -5,7 +5,7 @@ - + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml index b562f12d3..75b777fa5 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml @@ -5,7 +5,7 @@ - + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml index 2500c68ae..7b0d52e28 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml @@ -5,7 +5,7 @@ - + From d91db0af6fb955a1df247dbff8f1e0071033870b Mon Sep 17 00:00:00 2001 From: Fredrik Larrson Date: Mon, 8 Sep 2014 02:31:02 +1000 Subject: [PATCH 50/74] Revert "Merge remote-tracking branch 'origin/proff/OP-1373_DSMFixes' into next" This reverts commit a39e3932748fa1ca0126a33c188703944e1e7c3e, reversing changes made to ebfdaf28b5295f558baf9297fdc8421d93214d83. --- flight/pios/inc/pios_dsm_priv.h | 3 +- flight/pios/stm32f4xx/pios_dsm.c | 41 ++++++++++++++----- .../coptercontrol/firmware/pios_board.c | 10 ++--- .../boards/revolution/firmware/pios_board.c | 20 +++------ .../plugins/setupwizard/connectiondiagram.cpp | 3 +- .../plugins/setupwizard/pages/inputpage.cpp | 18 +++----- .../src/plugins/setupwizard/setupwizard.cpp | 11 ++--- .../vehicleconfigurationhelper.cpp | 17 +++----- .../setupwizard/vehicleconfigurationsource.h | 2 +- shared/uavobjectdefinition/hwsettings.xml | 16 ++++---- 10 files changed, 65 insertions(+), 76 deletions(-) diff --git a/flight/pios/inc/pios_dsm_priv.h b/flight/pios/inc/pios_dsm_priv.h index d823b4f3d..1cdf88f75 100644 --- a/flight/pios/inc/pios_dsm_priv.h +++ b/flight/pios/inc/pios_dsm_priv.h @@ -111,8 +111,7 @@ /* DSM protocol variations */ enum pios_dsm_proto { - PIOS_DSM_PROTO_DSM210BIT, - PIOS_DSM_PROTO_DSM211BIT, + PIOS_DSM_PROTO_DSM2, PIOS_DSM_PROTO_DSMX10BIT, PIOS_DSM_PROTO_DSMX11BIT, }; diff --git a/flight/pios/stm32f4xx/pios_dsm.c b/flight/pios/stm32f4xx/pios_dsm.c index 70b32483d..b6d8f0d85 100644 --- a/flight/pios/stm32f4xx/pios_dsm.c +++ b/flight/pios/stm32f4xx/pios_dsm.c @@ -181,7 +181,7 @@ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) { struct pios_dsm_state *state = &(dsm_dev->state); - uint8_t resolution=10; + uint8_t resolution; #ifdef DSM_LOST_FRAME_COUNTER /* increment the lost frame counter */ @@ -191,16 +191,35 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) #endif /* check the frame type assuming master satellite stream */ - - switch(dsm_dev->proto) { - case PIOS_DSM_PROTO_DSM210BIT: - case PIOS_DSM_PROTO_DSMX10BIT: - resolution = 10; - break; - case PIOS_DSM_PROTO_DSM211BIT: - case PIOS_DSM_PROTO_DSMX11BIT: - resolution = 11; - break; + uint8_t type = state->received_data[1]; + switch (type) { + case 0x01: + case 0x02: + case 0x12: + /* DSM2, DSMJ stream */ + if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) { + /* DSM2/DSMJ resolution is known from the header */ + resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10; + } else { + /* DSMX resolution should explicitly be selected */ + goto stream_error; + } + break; + case 0xA2: + case 0xB2: + /* DSMX stream */ + if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) { + resolution = 10; + } else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) { + resolution = 11; + } else { + /* DSMX resolution should explicitly be selected */ + goto stream_error; + } + break; + default: + /* unknown yet data stream */ + goto stream_error; } /* unroll channels */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index 35029acd6..2cb1734ff 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -477,19 +477,15 @@ void PIOS_Board_Init(void) } #endif /* PIOS_INCLUDE_GPS */ break; - case HWSETTINGS_CC_MAINPORT_DSM210BIT: - case HWSETTINGS_CC_MAINPORT_DSM211BIT: + case HWSETTINGS_CC_MAINPORT_DSM2: case HWSETTINGS_CC_MAINPORT_DSMX10BIT: case HWSETTINGS_CC_MAINPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) { enum pios_dsm_proto proto; switch (hwsettings_cc_mainport) { - case HWSETTINGS_CC_MAINPORT_DSM210BIT: - proto = PIOS_DSM_PROTO_DSM210BIT; - break; - case HWSETTINGS_CC_MAINPORT_DSM211BIT: - proto = PIOS_DSM_PROTO_DSM211BIT; + case HWSETTINGS_CC_MAINPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; break; case HWSETTINGS_CC_MAINPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 73fb74db7..b26f75a8c 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -632,18 +632,14 @@ void PIOS_Board_Init(void) } #endif break; - case HWSETTINGS_RM_MAINPORT_DSM210BIT: - case HWSETTINGS_RM_MAINPORT_DSM211BIT: + case HWSETTINGS_RM_MAINPORT_DSM2: case HWSETTINGS_RM_MAINPORT_DSMX10BIT: case HWSETTINGS_RM_MAINPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hwsettings_mainport) { - case HWSETTINGS_RM_MAINPORT_DSM210BIT: - proto = PIOS_DSM_PROTO_DSM210BIT; - break; - case HWSETTINGS_RM_MAINPORT_DSM211BIT: - proto = PIOS_DSM_PROTO_DSM211BIT; + case HWSETTINGS_RM_MAINPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; break; case HWSETTINGS_RM_MAINPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; @@ -705,18 +701,14 @@ void PIOS_Board_Init(void) 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); break; - case HWSETTINGS_RM_FLEXIPORT_DSM210BIT: - case HWSETTINGS_RM_FLEXIPORT_DSM211BIT: + case HWSETTINGS_RM_FLEXIPORT_DSM2: case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hwsettings_flexiport) { - case HWSETTINGS_RM_FLEXIPORT_DSM210BIT: - proto = PIOS_DSM_PROTO_DSM210BIT; - break; - case HWSETTINGS_RM_FLEXIPORT_DSM211BIT: - proto = PIOS_DSM_PROTO_DSM211BIT; + case HWSETTINGS_RM_FLEXIPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; break; case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp index 8b1f56d92..22b85bdd3 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp @@ -139,8 +139,7 @@ void ConnectionDiagram::setupGraphicsScene() break; case VehicleConfigurationSource::INPUT_DSMX10: case VehicleConfigurationSource::INPUT_DSMX11: - case VehicleConfigurationSource::INPUT_DSM2_10: - case VehicleConfigurationSource::INPUT_DSM2_11: + case VehicleConfigurationSource::INPUT_DSM2: elementsToShow << "satellite"; break; default: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp index 5c9ec16b7..5515a31dd 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp @@ -54,7 +54,7 @@ bool InputPage::validatePage() } else if (ui->sbusButton->isChecked()) { getWizard()->setInputType(SetupWizard::INPUT_SBUS); } else if (ui->spectrumButton->isChecked()) { - getWizard()->setInputType(SetupWizard::INPUT_DSM2_11); + getWizard()->setInputType(SetupWizard::INPUT_DSM2); } else { getWizard()->setInputType(SetupWizard::INPUT_PWM); } @@ -85,13 +85,9 @@ bool InputPage::restartNeeded(VehicleConfigurationSource::INPUT_TYPE selectedTyp case VehicleConfigurationSource::INPUT_SBUS: return data.CC_MainPort != HwSettings::CC_MAINPORT_SBUS; - case VehicleConfigurationSource::INPUT_DSM2_10: + case VehicleConfigurationSource::INPUT_DSM2: // TODO: Handle all of the DSM types ?? Which is most common? - return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM210BIT; - - case VehicleConfigurationSource::INPUT_DSM2_11: - // TODO: Handle all of the DSM types ?? Which is most common? - return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM211BIT; + return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM2; default: return true; } @@ -109,13 +105,9 @@ bool InputPage::restartNeeded(VehicleConfigurationSource::INPUT_TYPE selectedTyp case VehicleConfigurationSource::INPUT_SBUS: return data.RM_MainPort != HwSettings::CC_MAINPORT_SBUS; - case VehicleConfigurationSource::INPUT_DSM2_10: + case VehicleConfigurationSource::INPUT_DSM2: // TODO: Handle all of the DSM types ?? Which is most common? - return data.RM_MainPort != HwSettings::CC_MAINPORT_DSM210BIT; - - case VehicleConfigurationSource::INPUT_DSM2_11: - // TODO: Handle all of the DSM types ?? Which is most common? - return data.RM_MainPort != HwSettings::CC_MAINPORT_DSM211BIT; + return data.RM_MainPort != HwSettings::CC_MAINPORT_DSM2; default: return true; } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index cc2cfd253..1657ca526 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -252,17 +252,14 @@ QString SetupWizard::getSummaryText() case INPUT_SBUS: summary.append(tr("Futaba S.Bus")); break; - case INPUT_DSM2_10: - summary.append(tr("Spektrum satellite (DSM2 10bits)")); - break; - case INPUT_DSM2_11: - summary.append(tr("Spektrum satellite (DSM2 11bits)")); + case INPUT_DSM2: + summary.append(tr("Spektrum satellite (DSM2)")); break; case INPUT_DSMX10: - summary.append(tr("Spektrum satellite (DSMX 10bits)")); + summary.append(tr("Spektrum satellite (DSMX10BIT)")); break; case INPUT_DSMX11: - summary.append(tr("Spektrum satellite (DSMX 11bits)")); + summary.append(tr("Spektrum satellite (DSMX11BIT)")); break; default: summary.append(tr("Unknown")); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 6357670c3..4fd26406e 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -140,11 +140,8 @@ void VehicleConfigurationHelper::applyHardwareConfiguration() case VehicleConfigurationSource::INPUT_DSMX11: data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSMX11BIT; break; - case VehicleConfigurationSource::INPUT_DSM2_10: - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM210BIT; - break; - case VehicleConfigurationSource::INPUT_DSM2_11: - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM211BIT; + case VehicleConfigurationSource::INPUT_DSM2: + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM2; break; default: break; @@ -176,11 +173,8 @@ void VehicleConfigurationHelper::applyHardwareConfiguration() case VehicleConfigurationSource::INPUT_DSMX11: data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSMX11BIT; break; - case VehicleConfigurationSource::INPUT_DSM2_10: - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM210BIT; - break; - case VehicleConfigurationSource::INPUT_DSM2_11: - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM211BIT; + case VehicleConfigurationSource::INPUT_DSM2: + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM2; break; default: break; @@ -563,8 +557,7 @@ void VehicleConfigurationHelper::applyManualControlDefaults() break; case VehicleConfigurationSource::INPUT_DSMX10: case VehicleConfigurationSource::INPUT_DSMX11: - case VehicleConfigurationSource::INPUT_DSM2_10: - case VehicleConfigurationSource::INPUT_DSM2_11: + case VehicleConfigurationSource::INPUT_DSM2: channelType = ManualControlSettings::CHANNELGROUPS_DSMMAINPORT; break; default: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index 3c0399421..797283656 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -62,7 +62,7 @@ public: MULTI_ROTOR_OCTO_X, MULTI_ROTOR_OCTO_V, MULTI_ROTOR_OCTO_COAX_X, MULTI_ROTOR_OCTO_COAX_PLUS, FIXED_WING_AILERON, FIXED_WING_VTAIL, HELI_CCPM }; enum ESC_TYPE { ESC_RAPID, ESC_LEGACY, ESC_UNKNOWN }; - enum INPUT_TYPE { INPUT_PWM, INPUT_PPM, INPUT_SBUS, INPUT_DSMX10, INPUT_DSMX11, INPUT_DSM2_10, INPUT_DSM2_11, INPUT_UNKNOWN }; + enum INPUT_TYPE { INPUT_PWM, INPUT_PPM, INPUT_SBUS, INPUT_DSMX10, INPUT_DSMX11, INPUT_DSM2, INPUT_UNKNOWN }; enum GPS_SETTING { GPS_UBX, GPS_NMEA, GPS_DISABLED }; enum RADIO_SETTING { RADIO_TELEMETRY, RADIO_DISABLED }; diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index b0a8def85..7fa821da4 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -2,18 +2,20 @@ Selection of optional hardware configurations. - - + + - - - + + + + - - + + + From 65e0124a743b7cda418a0211cd653d05cc19b8f2 Mon Sep 17 00:00:00 2001 From: Fredrik Larsson Date: Mon, 8 Sep 2014 18:19:03 +1000 Subject: [PATCH 51/74] Remove QtCreator user file from git --- .../src/libs/sdlgamepad/sdlgamepad.pro.user | 112 ------------------ 1 file changed, 112 deletions(-) delete mode 100644 ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.pro.user diff --git a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.pro.user b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.pro.user deleted file mode 100644 index 96217e1ea..000000000 --- a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.pro.user +++ /dev/null @@ -1,112 +0,0 @@ - - - - ProjectExplorer.Project.ActiveTarget - 0 - - - ProjectExplorer.Project.EditorSettings - - System - - - - ProjectExplorer.Project.Target.0 - - Desktop - Qt4ProjectManager.Target.DesktopTarget - 0 - 0 - - - qmake - QtProjectManager.QMakeBuildStep - - - - Make - Qt4ProjectManager.MakeStep - false - - - - 2 - - Make - Qt4ProjectManager.MakeStep - true - - clean - - - - 1 - false - - Debug - Qt4ProjectManager.Qt4BuildConfiguration - 2 - /Users/jcotton81/Documents/Programming/OpenPilot/ground/src/plugins/gcscontrol/sdlgamepad-build-desktop - 2 - 0 - true - - - - qmake - QtProjectManager.QMakeBuildStep - - - - Make - Qt4ProjectManager.MakeStep - false - - - - 2 - - Make - Qt4ProjectManager.MakeStep - true - - clean - - - - 1 - false - - Release - Qt4ProjectManager.Qt4BuildConfiguration - 0 - /Users/jcotton81/Documents/Programming/OpenPilot/ground/src/plugins/gcscontrol/sdlgamepad-build-desktop - 2 - 0 - true - - 2 - - - 2 - - false - - - false - $BUILDDIR - Custom Executable - ProjectExplorer.CustomExecutableRunConfiguration - - 1 - - - - ProjectExplorer.Project.TargetCount - 1 - - - ProjectExplorer.Project.Updater.FileVersion - 4 - - From d04b8fecfbb5fbda30d85a7fd1a74c0d9b0c2027 Mon Sep 17 00:00:00 2001 From: Fredrik Larsson Date: Mon, 8 Sep 2014 18:23:03 +1000 Subject: [PATCH 52/74] Update gitignore to make sure we keep that pro.user out of git --- .gitignore | 145 +++++++++++++++++++++++++++-------------------------- 1 file changed, 73 insertions(+), 72 deletions(-) diff --git a/.gitignore b/.gitignore index b2583f3b1..bab962c2b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,72 +1,73 @@ -# Ignore artifacts of top-level Makefile -/downloads -/tools -/build - -# Exclude temporary and system files -Thumbs.db -.DS_Store -GPATH -GRTAGS -GSYMS -GTAGS -core - -# flight -/flight/*.pnproj -/flight/*.pnps -/flight/.cproject -/flight/.metadata -/flight/.project -/flight/.settings -/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/*.mode1v3 -/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/*.pbxuser - -# ground -openpilotgcs-build-desktop -ground/openpilotgcs/.cproject -ground/openpilotgcs/.project -ground/openpilotgcs/.settings - -# Ignore some of the .pro.user files -*.pro.user -/ground/openpilotgcs/openpilotgcs.pro.user -/ground/uavobjgenerator/uavobjgenerator.pro.user -/ground/uavobjects/uavobjects.pro.user -/ground/ground.pro.user - -# Misc artifacts -/ground/openpilotgcs/share/openpilotgcs/sounds/normalize.exe -/ground/openpilotgcs/share/openpilotgcs/sounds/default/normalize.exe -/ground/openpilotgcs/share/openpilotgcs/translations/extract-mimetypes.xq -/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/ui_mainwindow.h -/ground/openpilotgcs/src/libs/qextserialport/.hg -/ground/openpilotgcs/src/libs/qextserialport/.hgtags -/ground/openpilotgcs/src/libs/qwt/qwt.prf -/ground/openpilotgcs/src/libs/qwt/designer -/ground/openpilotgcs/src/plugins/ipconnection/ui_ipconnectionoptionspage.h - -# Ignore intermediate files generated by command-line android builds -# Couldn't figure out how to force these files into the ./build directory -/androidgcs/build.xml -/androidgcs/local.properties -/androidgcs/proguard-project.txt - -# Ignore build output from Eclipse android builds -/androidgcs/bin/ -/androidgcs/gen/ - -# Ignore Eclipse Projects and Metadata -/.cproject -/.project -/.metadata -/.settings -/.pydevproject - -# Ignore Eclipse temp folder, git plugin based? -RemoteSystemsTempFiles - -# Ignore patch-related files -*.rej -*.orig -*.diff~ +# Ignore artifacts of top-level Makefile +/downloads +/tools +/build + +# Exclude temporary and system files +Thumbs.db +.DS_Store +GPATH +GRTAGS +GSYMS +GTAGS +core + +# flight +/flight/*.pnproj +/flight/*.pnps +/flight/.cproject +/flight/.metadata +/flight/.project +/flight/.settings +/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/*.mode1v3 +/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/*.pbxuser + +# ground +openpilotgcs-build-desktop +ground/openpilotgcs/.cproject +ground/openpilotgcs/.project +ground/openpilotgcs/.settings + +# Ignore some of the .pro.user files +*.pro.user +/ground/openpilotgcs/openpilotgcs.pro.user +/ground/uavobjgenerator/uavobjgenerator.pro.user +/ground/uavobjects/uavobjects.pro.user +/ground/ground.pro.user +/ground/openpilotgcs/src/libs/sdlgamepad.pro.user + +# Misc artifacts +/ground/openpilotgcs/share/openpilotgcs/sounds/normalize.exe +/ground/openpilotgcs/share/openpilotgcs/sounds/default/normalize.exe +/ground/openpilotgcs/share/openpilotgcs/translations/extract-mimetypes.xq +/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/ui_mainwindow.h +/ground/openpilotgcs/src/libs/qextserialport/.hg +/ground/openpilotgcs/src/libs/qextserialport/.hgtags +/ground/openpilotgcs/src/libs/qwt/qwt.prf +/ground/openpilotgcs/src/libs/qwt/designer +/ground/openpilotgcs/src/plugins/ipconnection/ui_ipconnectionoptionspage.h + +# Ignore intermediate files generated by command-line android builds +# Couldn't figure out how to force these files into the ./build directory +/androidgcs/build.xml +/androidgcs/local.properties +/androidgcs/proguard-project.txt + +# Ignore build output from Eclipse android builds +/androidgcs/bin/ +/androidgcs/gen/ + +# Ignore Eclipse Projects and Metadata +/.cproject +/.project +/.metadata +/.settings +/.pydevproject + +# Ignore Eclipse temp folder, git plugin based? +RemoteSystemsTempFiles + +# Ignore patch-related files +*.rej +*.orig +*.diff~ From 67e7801949b66062589cb68ca81a94a045babf23 Mon Sep 17 00:00:00 2001 From: James Duley Date: Mon, 8 Sep 2014 21:29:44 +1200 Subject: [PATCH 53/74] OP-1478: move TOOLS_DIR out of linux scope --- ground/openpilotgcs/openpilotgcs.pri | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/openpilotgcs.pri b/ground/openpilotgcs/openpilotgcs.pri index 577e44db9..be23a53a8 100644 --- a/ground/openpilotgcs/openpilotgcs.pri +++ b/ground/openpilotgcs/openpilotgcs.pri @@ -72,6 +72,10 @@ isEmpty(GCS_BUILD_TREE) { GCS_BUILD_TREE = $$cleanPath($$OUT_PWD) GCS_BUILD_TREE ~= s,$$re_escape($$sub_dir)$,, } + +TOOLS_DIR = $$(OPENPILOT_TOOLS_DIR) +isEmpty(TOOLS_DIR):TOOLS_DIR = $$clean_path($$GCS_SOURCE_TREE/../../tools) + GCS_APP_PATH = $$GCS_BUILD_TREE/bin macx { GCS_APP_TARGET = "OpenPilot GCS" @@ -97,8 +101,6 @@ macx { GCS_QT_PLUGINS_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/plugins GCS_QT_QML_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/qml - TOOLS_DIR = $$(OPENPILOT_TOOLS_DIR) - isEmpty(TOOLS_DIR):TOOLS_DIR = $$clean_path($$GCS_SOURCE_TREE/../../tools) QT_INSTALL_DIR = $$clean_path($$[QT_INSTALL_LIBS]/../../../..) equals(QT_INSTALL_DIR, $$TOOLS_DIR) { copyqt = 1 From 2907af618f7509a1f03d3f51301bc39764fa7aea Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 8 Sep 2014 14:39:41 +0200 Subject: [PATCH 54/74] OP-1465 - fixes for some review suggestions --- make/tools.mk | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/make/tools.mk b/make/tools.mk index afb1349dc..8303bd126 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -1101,21 +1101,21 @@ stm32flash_install: STM32FLASH_URL := https://code.google.com/p/stm32flash/ stm32flash_install: STM32FLASH_REV := a358bd1f025d stm32flash_install: stm32flash_clean # download the source - $(V0) @echo " DOWNLOAD $(STM32FLASH_URL) @ r$(STM32FLASH_REV)" + $(V0) @$(ECHO) " DOWNLOAD $(STM32FLASH_URL) @ r$(STM32FLASH_REV)" $(V1) [ ! -d "$(STM32FLASH_DIR)" ] || $(RM) -rf "$(STM32FLASH_DIR)" - $(V1) mkdir -p "$(STM32FLASH_DIR)" - $(V1) git clone --no-checkout $(STM32FLASH_URL) "$(STM32FLASH_DIR)" + $(V1) $(MKDIR) -p "$(STM32FLASH_DIR)" + $(V1) $(GIT) clone --no-checkout $(STM32FLASH_URL) "$(STM32FLASH_DIR)" $(V1) ( \ - cd $(STM32FLASH_DIR) ; \ - git checkout -q $(STM32FLASH_REV) ; \ + $(CD) $(STM32FLASH_DIR) ; \ + $(GIT) checkout -q $(STM32FLASH_REV) ; \ ) # build - $(V0) @echo " BUILD $(STM32FLASH_DIR)" + $(V0) @$(ECHO) " BUILD $(STM32FLASH_DIR)" $(V1) $(MAKE) --silent -C $(STM32FLASH_DIR) all $(STM32FLASH_BUILD_OPTIONS) .PHONY: stm32flash_clean stm32flash_clean: - $(V0) @echo " CLEAN $(STM32FLASH_DIR)" + $(V0) @$(ECHO) " CLEAN $(STM32FLASH_DIR)" $(V1) [ ! -d "$(STM32FLASH_DIR)" ] || $(RM) -rf "$(STM32FLASH_DIR)" DFUUTIL_DIR := $(TOOLS_DIR)/dfu-util From c777f9e1943e23ab20e510577b5b892ec1e2d020 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 8 Sep 2014 23:24:18 +0200 Subject: [PATCH 55/74] OP-1406 - Fixes from review comments --- .../modules/StateEstimation/stateestimation.c | 2 +- .../calibration/sixpointcalibrationmodel.cpp | 18 +++++++++--------- .../calibration/sixpointcalibrationmodel.h | 12 +++++++----- shared/uavobjectdefinition/magstate.xml | 2 +- 4 files changed, 18 insertions(+), 16 deletions(-) diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index c3c105c53..cf33bbbc4 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -483,7 +483,7 @@ static void StateEstimationCb(void) s.z = states.mag[2]; switch (states.magStatus) { case MAGSTATUS_OK: - s.Source = MAGSTATE_SOURCE_OK; + s.Source = MAGSTATE_SOURCE_ONBOARD; break; case MAGSTATUS_AUX: s.Source = MAGSTATE_SOURCE_AUX; diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp index 526a2a9a4..f877a44b5 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp @@ -45,7 +45,7 @@ namespace OpenPilot { SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) : QObject(parent), calibratingMag(false), - externalMagAvailable(false), + calibratingAuxMag(false), calibratingAccel(false), calibrationStepsMag(), calibrationStepsAccelOnly(), @@ -334,7 +334,7 @@ void SixPointCalibrationModel::getSample(UAVObject *obj) aux_mag_accum_x.append(auxMagData.x); aux_mag_accum_y.append(auxMagData.y); aux_mag_accum_z.append(auxMagData.z); - externalMagAvailable = true; + calibratingAuxMag = true; #ifndef FITTING_USING_CONTINOUS_ACQUISITION aux_mag_fit_x.append(auxMagData.x); aux_mag_fit_y.append(auxMagData.y); @@ -425,7 +425,7 @@ void SixPointCalibrationModel::continouslyGetMagSamples(UAVObject *obj) aux_mag_fit_x.append(auxMagData.x); aux_mag_fit_y.append(auxMagData.y); aux_mag_fit_z.append(auxMagData.z); - externalMagAvailable = true; + calibratingAuxMag = true; } } } @@ -463,10 +463,10 @@ void SixPointCalibrationModel::compute() qDebug() << "-----------------------------------"; qDebug() << "Onboard Mag"; - CalcCalibration(mag_fit_x, mag_fit_y, mag_fit_z, Be_length, revoCalibrationData.mag_transform, revoCalibrationData.mag_bias); - if (externalMagAvailable) { + 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); + calcCalibration(aux_mag_fit_x, aux_mag_fit_y, aux_mag_fit_z, Be_length, auxCalibrationData.mag_transform, auxCalibrationData.mag_bias); } } // Restore the previous setting @@ -491,7 +491,7 @@ 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 (externalMagAvailable) { + 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]); @@ -537,7 +537,7 @@ void SixPointCalibrationModel::compute() position = -1; } -void SixPointCalibrationModel::CalcCalibration(QList x, QList y, QList z, double Be_length, float calibrationMatrix[], float bias[]) +void SixPointCalibrationModel::calcCalibration(QList x, QList y, QList z, double Be_length, float calibrationMatrix[], float bias[]) { int vectSize = x.count(); Eigen::VectorXf samples_x(vectSize); @@ -589,7 +589,7 @@ void SixPointCalibrationModel::save() } revoCalibration->setData(revoCalibrationData); - if (externalMagAvailable) { + 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++) { diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h index 368762e7e..b398aeb0c 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h @@ -104,7 +104,7 @@ public: } Result; bool calibratingMag; - bool externalMagAvailable; + bool calibratingAuxMag; bool calibratingAccel; QList calibrationStepsMag; @@ -125,15 +125,17 @@ public: QList accel_accum_x; QList accel_accum_y; QList accel_accum_z; + QList mag_accum_x; QList mag_accum_y; QList mag_accum_z; - QList aux_mag_accum_x; - QList aux_mag_accum_y; - QList aux_mag_accum_z; QList mag_fit_x; QList mag_fit_y; QList mag_fit_z; + + QList aux_mag_accum_x; + QList aux_mag_accum_y; + QList aux_mag_accum_z; QList aux_mag_fit_x; QList aux_mag_fit_y; QList aux_mag_fit_z; @@ -152,7 +154,7 @@ public: void compute(); void showHelp(QString image); UAVObjectManager *getObjectManager(); - void CalcCalibration(QList x, QList y, QList z, double Be_length, float calibrationMatrix[], float bias[]); + void calcCalibration(QList x, QList y, QList z, double Be_length, float calibrationMatrix[], float bias[]); }; } diff --git a/shared/uavobjectdefinition/magstate.xml b/shared/uavobjectdefinition/magstate.xml index 49a720fed..0de27ce17 100644 --- a/shared/uavobjectdefinition/magstate.xml +++ b/shared/uavobjectdefinition/magstate.xml @@ -4,7 +4,7 @@ - + From 10703aaf463302ceea22633248f5e8edcbcfa2b3 Mon Sep 17 00:00:00 2001 From: Fredrik Larsson Date: Wed, 10 Sep 2014 04:16:40 +1000 Subject: [PATCH 56/74] Update Miletones --- MILESTONES.txt | 55 ++++++++++++++++++++++++++++++++++---------------- 1 file changed, 38 insertions(+), 17 deletions(-) diff --git a/MILESTONES.txt b/MILESTONES.txt index 4480129b1..e72a543a0 100644 --- a/MILESTONES.txt +++ b/MILESTONES.txt @@ -177,15 +177,51 @@ C: Eric Price (Corvus Corax) D: March 2012 V: +M: First Auto spot landing on a fixed Wing using Revo +C: Eric Price (Corvus Corax) +D: Greece 2013 +V: + M: First Revo Navigated flight on a MultiRotor C: It got done somewhere along the line, James or Sami M: First Revo 1km Navigated flight on a MultiRotor +C: Jackson Russell +D: Greece 2013 +V: + +M: First Revo 5km Navigated flight on a MultiRotor +C: RoddersNZ +D: September 2014 +V: https://www.youtube.com/watch?v=DYawRGz5KYM + +M: First Revo 6km Navigated flight on a MultiRotor C: D: V: -M: First Revo 5km Navigated flight on a MultiRotor +M: First Revo 7km Navigated flight on a MultiRotor +C: +D: +V: + +M: First Revo 8km Navigated flight on a MultiRotor +C: +D: +V: + +M: First Revo 9km Navigated flight on a MultiRotor +C: +D: +V: + +M: First Revo 10km Navigated flight on a MultiRotor +C: +D: +V: + + +M: First Revo Position Hold on a Heli C: D: V: @@ -205,23 +241,8 @@ C: D: V: -M: First Auto spot landing on a fixed Wing using Revo -C: -D: -V: - -M: First Auto take-off on a MultiRotor using Revo -C: -D: -V: - M: First Auto landing on a MultiRotor using Revo -C: Sami (please complete details) -D: -V: - -M: First Auto take-off on a Heli using Revo -C: +C: Sami D: V: From f6b097ee77a129051cee2eb62842160050ee4024 Mon Sep 17 00:00:00 2001 From: Fredrik Larsson Date: Wed, 10 Sep 2014 04:24:06 +1000 Subject: [PATCH 57/74] Add missing 20km flight --- MILESTONES.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/MILESTONES.txt b/MILESTONES.txt index e72a543a0..c165cfe5f 100644 --- a/MILESTONES.txt +++ b/MILESTONES.txt @@ -177,6 +177,11 @@ C: Eric Price (Corvus Corax) D: March 2012 V: +M: First Revo 20km Navigated flight on a FixedWing +C: Eric Price / Team OP +D: Greece 2013 +V: https://vimeo.com/71956880 + M: First Auto spot landing on a fixed Wing using Revo C: Eric Price (Corvus Corax) D: Greece 2013 From 34c3706dedcc7e6cfb870ac4cae9dcf5f2928266 Mon Sep 17 00:00:00 2001 From: Fredrik Larsson Date: Thu, 11 Sep 2014 17:23:20 +1000 Subject: [PATCH 58/74] Use Rod's real name --- MILESTONES.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MILESTONES.txt b/MILESTONES.txt index c165cfe5f..f822f4ab1 100644 --- a/MILESTONES.txt +++ b/MILESTONES.txt @@ -196,7 +196,7 @@ D: Greece 2013 V: M: First Revo 5km Navigated flight on a MultiRotor -C: RoddersNZ +C: Rodney Grainger (roddersNZ) D: September 2014 V: https://www.youtube.com/watch?v=DYawRGz5KYM From ea64284d7de7ac2ed39f0ff77fd6d303a5e28ff8 Mon Sep 17 00:00:00 2001 From: James Duley Date: Sat, 13 Sep 2014 15:24:52 +1200 Subject: [PATCH 59/74] OP-1478: added a qmake variable SDL_DIR --- ground/openpilotgcs/openpilotgcs.pri | 2 ++ ground/openpilotgcs/src/libs/sdlgamepad/copydata.pro | 2 +- ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.pro | 4 ++-- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/openpilotgcs.pri b/ground/openpilotgcs/openpilotgcs.pri index be23a53a8..02a4d7b18 100644 --- a/ground/openpilotgcs/openpilotgcs.pri +++ b/ground/openpilotgcs/openpilotgcs.pri @@ -76,6 +76,8 @@ isEmpty(GCS_BUILD_TREE) { TOOLS_DIR = $$(OPENPILOT_TOOLS_DIR) isEmpty(TOOLS_DIR):TOOLS_DIR = $$clean_path($$GCS_SOURCE_TREE/../../tools) +SDL_DIR = $${TOOLS_DIR}/SDL-1.2.15 + GCS_APP_PATH = $$GCS_BUILD_TREE/bin macx { GCS_APP_TARGET = "OpenPilot GCS" diff --git a/ground/openpilotgcs/src/libs/sdlgamepad/copydata.pro b/ground/openpilotgcs/src/libs/sdlgamepad/copydata.pro index 1f99c0a48..4fa7cb219 100644 --- a/ground/openpilotgcs/src/libs/sdlgamepad/copydata.pro +++ b/ground/openpilotgcs/src/libs/sdlgamepad/copydata.pro @@ -5,7 +5,7 @@ equals(copydata, 1) { SDL_DLLS = \ SDL.dll for(dll, SDL_DLLS) { - data_copy.commands += $(COPY_FILE) $$targetPath(\"$$(SDL_DIR)/bin/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline() + data_copy.commands += $(COPY_FILE) $$targetPath(\"$${SDL_DIR}/bin/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline() } # add make target diff --git a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.pro b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.pro index 1a6a2a60d..92633eed7 100644 --- a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.pro +++ b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.pro @@ -37,8 +37,8 @@ macx { } win32 { - INCLUDEPATH += $(SDL_DIR)/include - LIBS += -L$(SDL_DIR)/lib + INCLUDEPATH += $${SDL_DIR}/include + LIBS += -L$${SDL_DIR}/lib } !mac:LIBS += -lSDL From 17bb1d2fc03ff1ac5e8e7588e48b971cf93b85f0 Mon Sep 17 00:00:00 2001 From: James Duley Date: Sat, 13 Sep 2014 16:37:30 +1200 Subject: [PATCH 60/74] OP-1478 fix win qtc vairables: fixed TOOLS_DIR check --- ground/openpilotgcs/openpilotgcs.pri | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/openpilotgcs.pri b/ground/openpilotgcs/openpilotgcs.pri index 02a4d7b18..8ad271b81 100644 --- a/ground/openpilotgcs/openpilotgcs.pri +++ b/ground/openpilotgcs/openpilotgcs.pri @@ -73,8 +73,15 @@ isEmpty(GCS_BUILD_TREE) { GCS_BUILD_TREE ~= s,$$re_escape($$sub_dir)$,, } -TOOLS_DIR = $$(OPENPILOT_TOOLS_DIR) -isEmpty(TOOLS_DIR):TOOLS_DIR = $$clean_path($$GCS_SOURCE_TREE/../../tools) +# Find the tools directory, +# try from Makefile (not run by Qt Creator), +TOOLS_DIR = $$(TOOLS_DIR) +isEmpty(TOOLS_DIR) { + # check for custom enviroment variable, + TOOLS_DIR = $$(OPENPILOT_TOOLS_DIR) + # fallback to default location. + isEmpty(TOOLS_DIR):TOOLS_DIR = $$clean_path($$GCS_SOURCE_TREE/../../tools) +} SDL_DIR = $${TOOLS_DIR}/SDL-1.2.15 From 684180ef87d83c88d23ae4fd01916641ca6c62b4 Mon Sep 17 00:00:00 2001 From: James Duley Date: Sat, 13 Sep 2014 17:48:30 +1200 Subject: [PATCH 61/74] OP-1478: added a qmake variable OPENSSL_DIR --- ground/openpilotgcs/copydata.pro | 2 +- ground/openpilotgcs/openpilotgcs.pri | 1 + make/tools.mk | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/copydata.pro b/ground/openpilotgcs/copydata.pro index d7fb31c25..3dd79869e 100644 --- a/ground/openpilotgcs/copydata.pro +++ b/ground/openpilotgcs/copydata.pro @@ -233,7 +233,7 @@ GCS_LIBRARY_PATH ssleay32.dll \ libeay32.dll for(dll, OPENSSL_DLLS) { - data_copy.commands += $(COPY_FILE) $$targetPath(\"$$(OPENSSL_DIR)/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline() + data_copy.commands += $(COPY_FILE) $$targetPath(\"$${OPENSSL_DIR}/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline() } # copy OpenGL DLL diff --git a/ground/openpilotgcs/openpilotgcs.pri b/ground/openpilotgcs/openpilotgcs.pri index 8ad271b81..9e2541a84 100644 --- a/ground/openpilotgcs/openpilotgcs.pri +++ b/ground/openpilotgcs/openpilotgcs.pri @@ -84,6 +84,7 @@ isEmpty(TOOLS_DIR) { } SDL_DIR = $${TOOLS_DIR}/SDL-1.2.15 +OPENSSL_DIR = $${TOOLS_DIR}/openssl-1.0.1e-win32 GCS_APP_PATH = $$GCS_BUILD_TREE/bin macx { diff --git a/make/tools.mk b/make/tools.mk index d66dd388d..d990e19a7 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -101,6 +101,7 @@ endif GTEST_URL := http://wiki.openpilot.org/download/attachments/18612236/gtest-1.6.0.zip # Changing PYTHON_DIR, also update it in ground/openpilotgcs/src/python.pri +# Changing SDL_DIR or OPENSSL_DIR, also update it in ground/openpilotgcs/openpilotgcs.pri ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-4_8-2014q1 QT_SDK_DIR := $(TOOLS_DIR)/qt-5.3.1 MINGW_DIR := $(QT_SDK_DIR)/Tools/mingw48_32 From 76b9b1c40ff9df589d9d0d60dd33c8a9293c1d82 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 13 Sep 2014 16:14:34 +0200 Subject: [PATCH 62/74] OP-1406 - Create a support library to share common calibration and sample publishing logic --- flight/libraries/auxmagsupport.c | 72 +++++++++++++++++++ flight/libraries/inc/auxmagsupport.h | 51 +++++++++++++ .../boards/coptercontrol/firmware/Makefile | 1 + .../boards/discoveryf4bare/firmware/Makefile | 1 + flight/targets/boards/osd/firmware/Makefile | 1 + .../boards/revolution/firmware/Makefile | 1 + .../boards/revoproto/firmware/Makefile | 1 + 7 files changed, 128 insertions(+) create mode 100644 flight/libraries/auxmagsupport.c create mode 100644 flight/libraries/inc/auxmagsupport.h diff --git a/flight/libraries/auxmagsupport.c b/flight/libraries/auxmagsupport.c new file mode 100644 index 000000000..3d27e336a --- /dev/null +++ b/flight/libraries/auxmagsupport.c @@ -0,0 +1,72 @@ +/** + ****************************************************************************** + * + * @file auxmagsupport.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Functions to handle aux mag data and calibration. + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include +#include "inc/auxmagsupport.h" +#include "CoordinateConversions.h" + +static float mag_bias[3] = { 0, 0, 0 }; +static float mag_transform[3][3] = { + { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } +}; + +AuxMagSettingsTypeOptions option; + +void auxmagsupport_reload_settings() +{ + AuxMagSettingsTypeGet(&option); + float a[3][3]; + float b[3][3]; + float rotz; + AuxMagSettingsmag_transformArrayGet((float *)a); + AuxMagSettingsOrientationGet(&rotz); + rotz = DEG2RAD(rotz); + rot_about_axis_z(rotz, b); + matrix_mult_3x3f(a, b, mag_transform); + AuxMagSettingsmag_biasArrayGet(mag_bias); +} + +void auxmagsupport_publish_samples(float mags[3], uint8_t status) +{ + float mag_out[3]; + + mags[0] -= mag_bias[0]; + mags[1] -= mag_bias[1]; + mags[2] -= mag_bias[2]; + rot_mult(mag_transform, mags, mag_out); + + AuxMagSensorData data; + data.x = mag_out[0]; + data.y = mag_out[1]; + data.z = mag_out[2]; + data.Status = status; + AuxMagSensorSet(&data); +} + +AuxMagSettingsTypeOptions auxmagsupport_get_type() +{ + return option; +} diff --git a/flight/libraries/inc/auxmagsupport.h b/flight/libraries/inc/auxmagsupport.h new file mode 100644 index 000000000..90b063e9e --- /dev/null +++ b/flight/libraries/inc/auxmagsupport.h @@ -0,0 +1,51 @@ +/** + ****************************************************************************** + * + * @file auxmagsupport.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Functions to handle aux mag data and calibration. + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef AUXMAGSUPPORT_H_ +#define AUXMAGSUPPORT_H_ +#include +#include +#include +/** + * @brief reload Aux Mag settings + */ +void auxmagsupport_reload_settings(); + +/** + * @brief Publish a new Aux Magnetometer sample + * @param[in] mags Mag sample in milliGauss + * @param[in] status one of AuxMagSensorStatusOptions option + */ +void auxmagsupport_publish_samples(float mags[3], uint8_t status); + +/** + * @brief Get the Aux Mag settings Type option + * @param[in] mags Mag sample in milliGauss + * @return one of AuxMagSettingsTypeOptions option + */ +AuxMagSettingsTypeOptions auxmagsupport_get_type(); + + +#endif /* AUXMAGSUPPORT_H_ */ diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index bdcbfaed6..e476dbb85 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -76,6 +76,7 @@ ifndef TESTAPP SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(FLIGHTLIB)/auxmagsupport.c ## UAVObjects SRC += $(OPUAVSYNTHDIR)/accessorydesired.c diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index bc8e72959..bf7d21d28 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -86,6 +86,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/auxmagsupport.c ## UAVObjects include ./UAVObjects.inc diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile index 6ff4d37cb..6bc37c1d8 100644 --- a/flight/targets/boards/osd/firmware/Makefile +++ b/flight/targets/boards/osd/firmware/Makefile @@ -70,6 +70,7 @@ ifndef TESTAPP ## Misc library functions SRC += $(FLIGHTLIB)/WorldMagModel.c + SRC += $(FLIGHTLIB)/auxmagsupport.c ## UAVObjects SRC += $(OPUAVSYNTHDIR)/objectpersistence.c diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 52b5f8a64..21b23ed68 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -87,6 +87,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/auxmagsupport.c ## UAVObjects include ./UAVObjects.inc diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 9933124a3..f8f0f5336 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -85,6 +85,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/auxmagsupport.c ## UAVObjects include ./UAVObjects.inc From 3fd5ca08cdf7efb2d899b25d7506fb3114c8ea82 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 13 Sep 2014 16:15:02 +0200 Subject: [PATCH 63/74] OP-1434 - Switch to common aux mag support library --- flight/modules/GPS/UBX.c | 41 +++++----------------------------------- 1 file changed, 5 insertions(+), 36 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 6265262eb..ba417b5ac 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -30,20 +30,14 @@ #include "openpilot.h" #include "pios.h" -#include "CoordinateConversions.h" #include "pios_math.h" #include #include #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) #include "inc/UBX.h" #include "inc/GPS.h" -#include "auxmagsettings.h" #include - -static float mag_bias[3] = { 0, 0, 0 }; -static float mag_transform[3][3] = { - { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } -}; +#include static bool useMag = false; static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; @@ -485,20 +479,8 @@ static void parse_ubx_op_mag(struct UBXPacket *ubx, __attribute__((unused)) GPSP return; } struct UBX_OP_MAG *mag = &ubx->payload.op_mag; - float mags[3] = { mag->x - mag_bias[0], - mag->y - mag_bias[1], - mag->z - mag_bias[2] }; - - float mag_out[3]; - - 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 = mag->Status; - AuxMagSensorSet(&data); + float mags[3] = { mag->x, mag->y, mag->z }; + auxmagsupport_publish_samples(mags, AUXMAGSENSOR_STATUS_OK); } @@ -537,30 +519,17 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi return id; } - void load_mag_settings() { - AuxMagSettingsTypeOptions option; + auxmagsupport_reload_settings(); - AuxMagSettingsTypeGet(&option); - if (option != AUXMAGSETTINGS_TYPE_GPSV9) { + 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 { - 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); useMag = true; } } - - #endif // PIOS_INCLUDE_GPS_UBX_PARSER From 5d55766573fcad222da1bc15d02fdea94a786fe3 Mon Sep 17 00:00:00 2001 From: James Duley Date: Sun, 14 Sep 2014 11:55:32 +1200 Subject: [PATCH 64/74] OP-1478 fix_win_qtc_variables: reworded comments --- make/tools.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/make/tools.mk b/make/tools.mk index d990e19a7..1cd693595 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -100,8 +100,8 @@ endif GTEST_URL := http://wiki.openpilot.org/download/attachments/18612236/gtest-1.6.0.zip -# Changing PYTHON_DIR, also update it in ground/openpilotgcs/src/python.pri -# Changing SDL_DIR or OPENSSL_DIR, also update it in ground/openpilotgcs/openpilotgcs.pri +# When changing PYTHON_DIR, you must also update it in ground/openpilotgcs/src/python.pri +# When changing SDL_DIR or OPENSSL_DIR, you must also update them in ground/openpilotgcs/openpilotgcs.pri ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-4_8-2014q1 QT_SDK_DIR := $(TOOLS_DIR)/qt-5.3.1 MINGW_DIR := $(QT_SDK_DIR)/Tools/mingw48_32 From 18ad22a1770d7b3ded3065ad1a86bbdad51c66eb Mon Sep 17 00:00:00 2001 From: James Duley Date: Sun, 14 Sep 2014 12:01:14 +1200 Subject: [PATCH 65/74] OP-1478: allow alternate SDL_DIR or OPENSSL_DIR --- ground/openpilotgcs/openpilotgcs.pri | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/openpilotgcs.pri b/ground/openpilotgcs/openpilotgcs.pri index 9e2541a84..749c28353 100644 --- a/ground/openpilotgcs/openpilotgcs.pri +++ b/ground/openpilotgcs/openpilotgcs.pri @@ -83,9 +83,6 @@ isEmpty(TOOLS_DIR) { isEmpty(TOOLS_DIR):TOOLS_DIR = $$clean_path($$GCS_SOURCE_TREE/../../tools) } -SDL_DIR = $${TOOLS_DIR}/SDL-1.2.15 -OPENSSL_DIR = $${TOOLS_DIR}/openssl-1.0.1e-win32 - GCS_APP_PATH = $$GCS_BUILD_TREE/bin macx { GCS_APP_TARGET = "OpenPilot GCS" @@ -101,6 +98,12 @@ macx { } else { !isEqual(GCS_SOURCE_TREE, $$GCS_BUILD_TREE):copydata = 1 win32 { + SDL_DIR = $$(SDL_DIR) + isEmpty(SDL_DIR):SDL_DIR = $${TOOLS_DIR}/SDL-1.2.15 + + OPENSSL_DIR = $$(OPENSSL_DIR) + isEmpty(OPENSSL_DIR):OPENSSL_DIR = $${TOOLS_DIR}/openssl-1.0.1e-win32 + contains(TEMPLATE, vc.*)|contains(TEMPLATE_PREFIX, vc):vcproj = 1 GCS_APP_TARGET = openpilotgcs copyqt = $$copydata From 5cc91467ef514406911a5a4560a7150442510d2a Mon Sep 17 00:00:00 2001 From: James Duley Date: Sun, 14 Sep 2014 12:50:30 +1200 Subject: [PATCH 66/74] OP-1478 fix_win_qtc_variables: added MESAWIN_DIR variable --- ground/openpilotgcs/copydata.pro | 2 +- ground/openpilotgcs/openpilotgcs.pri | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/copydata.pro b/ground/openpilotgcs/copydata.pro index 3dd79869e..40721f15d 100644 --- a/ground/openpilotgcs/copydata.pro +++ b/ground/openpilotgcs/copydata.pro @@ -240,7 +240,7 @@ GCS_LIBRARY_PATH OPENGL_DLLS = \ opengl32_32/opengl32.dll for(dll, OPENGL_DLLS) { - data_copy.commands += $(COPY_FILE) $$targetPath(\"$$(MESAWIN_DIR)/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline() + data_copy.commands += $(COPY_FILE) $$targetPath(\"$${MESAWIN_DIR}/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline() } data_copy.target = FORCE diff --git a/ground/openpilotgcs/openpilotgcs.pri b/ground/openpilotgcs/openpilotgcs.pri index 749c28353..ea1481589 100644 --- a/ground/openpilotgcs/openpilotgcs.pri +++ b/ground/openpilotgcs/openpilotgcs.pri @@ -104,6 +104,9 @@ macx { OPENSSL_DIR = $$(OPENSSL_DIR) isEmpty(OPENSSL_DIR):OPENSSL_DIR = $${TOOLS_DIR}/openssl-1.0.1e-win32 + MESAWIN_DIR = $$(MESAWIN_DIR) + isEmpty(MESAWIN_DIR):MESAWIN_DIR = $${TOOLS_DIR}/mesawin + contains(TEMPLATE, vc.*)|contains(TEMPLATE_PREFIX, vc):vcproj = 1 GCS_APP_TARGET = openpilotgcs copyqt = $$copydata From 25c718aaa8f1d4f0b189549b41b6380a1f315d14 Mon Sep 17 00:00:00 2001 From: Fredrik Larsson Date: Sun, 14 Sep 2014 23:02:56 +1000 Subject: [PATCH 67/74] Add Jim's amazing nav flight --- MILESTONES.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/MILESTONES.txt b/MILESTONES.txt index f822f4ab1..62395e8b4 100644 --- a/MILESTONES.txt +++ b/MILESTONES.txt @@ -211,9 +211,9 @@ D: V: M: First Revo 8km Navigated flight on a MultiRotor -C: -D: -V: +C: Jim Allen Thibodaux +D: September 2014 +V: http://www.youtube.com/watch?v=oeJF8U2k9FA M: First Revo 9km Navigated flight on a MultiRotor C: From 2f0c43c6e56badc01e9170aad852ddd09e10e05b Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sun, 14 Sep 2014 17:40:28 +0200 Subject: [PATCH 68/74] Wrong I2C bus used for the onboard magnetometer --- flight/targets/boards/revoproto/firmware/pios_board.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index d748265bb..8a414ddbd 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -946,7 +946,7 @@ void PIOS_Board_Init(void) #endif #if defined(PIOS_INCLUDE_HMC5X83) - onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_pressure_adapter_id, 0); + onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_adapter_id, 0); #endif #if defined(PIOS_INCLUDE_MS5611) From 3e785cf929f6408638d5e626276bf6c5fc3407aa Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 14 Sep 2014 21:44:41 +0200 Subject: [PATCH 69/74] OP-1434 - Use vector_lengthf, fix a typo in a comment --- flight/modules/StateEstimation/filtermag.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index aded510ef..732cb0c5f 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -38,6 +38,7 @@ #include #include #include +#include // Private constants // @@ -82,8 +83,8 @@ 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); - // magBe holds the squared magnetic vector length (extpected) - this->magBe = sqrtf(this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1] + this->homeLocationBe[2] * this->homeLocationBe[2]); + // magBe holds the squared magnetic vector length (expected) + this->magBe = vector_lengthf(this->homeLocationBe, 3); this->invMagBe = 1.0f / this->magBe; RevoCalibrationGet(&this->revoCalibration); RevoSettingsGet(&this->revoSettings); @@ -195,7 +196,7 @@ static bool checkMagValidity(struct data *this, float error, bool setAlarms) static float getMagError(struct data *this, float mag[3]) { // vector norm - float magnitude = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]); + float magnitude = vector_lengthf(mag, 3); // absolute value of relative error against Be float error = fabsf(magnitude - this->magBe) * this->invMagBe; From 4ed8fb0188a4d9b1d6f8b9e467cad375157f6815 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Mon, 15 Sep 2014 09:00:44 +0200 Subject: [PATCH 70/74] OP-1488 fixed typo in uavobject.java.template --- .../src/libs/juavobjects/templates/uavobject.java.template | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/libs/juavobjects/templates/uavobject.java.template b/ground/openpilotgcs/src/libs/juavobjects/templates/uavobject.java.template index d720bf898..0d2237b08 100644 --- a/ground/openpilotgcs/src/libs/juavobjects/templates/uavobject.java.template +++ b/ground/openpilotgcs/src/libs/juavobjects/templates/uavobject.java.template @@ -80,7 +80,7 @@ $(FIELDSINIT) $(FLIGHTTELEM_ACKED) << UAVOBJ_TELEMETRY_ACKED_SHIFT | $(GCSTELEM_ACKED) << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT | UAVObject.Metadata.UpdateModeNum(UAVObject.UpdateMode.$(FLIGHTTELEM_UPDATEMODE)) << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT | - UAVObject.Metadata.UpdateModeNum(UAVObject.UpdateMode.$(GCSTELEM_UPDATEMODE)) << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT; + UAVObject.Metadata.UpdateModeNum(UAVObject.UpdateMode.$(GCSTELEM_UPDATEMODE)) << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT | UAVObject.Metadata.UpdateModeNum(UAVObject.UpdateMode.$(LOGGING_UPDATEMODE)) << UAVOBJ_LOGGING_UPDATE_MODE_SHIFT; metadata.flightTelemetryUpdatePeriod = $(FLIGHTTELEM_UPDATEPERIOD); metadata.gcsTelemetryUpdatePeriod = $(GCSTELEM_UPDATEPERIOD); From 55ef67e424286d3b6cc9004b6a3497f39e1e98f5 Mon Sep 17 00:00:00 2001 From: Fredrik Larsson Date: Mon, 15 Sep 2014 18:02:25 +1000 Subject: [PATCH 71/74] Update Credits to add new contributors --- CREDITS.txt | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/CREDITS.txt b/CREDITS.txt index 85368387f..adb5b9dd9 100644 --- a/CREDITS.txt +++ b/CREDITS.txt @@ -1,10 +1,11 @@ -Connor Abbott +Connor Abbott Sergiy Anikeyev David Ankers Fredrik Arvidsson Pedro Assuncao Werner Backes Jose Barros +Alex Beck Andre Bernet Mikael Blomqvist Pete Boehl @@ -13,11 +14,13 @@ Joel Brueziere Thierry Bugeat Glenn Campigli David Carlson +Mike Carr Stefan Cenkov Andrés Chavarría Krauser Cosimo Corrado James Cotton Steve Doll +James Duley Piotr Esden-Tempski Peter Farnworth Ed Faulkner @@ -25,6 +28,7 @@ Andrew Finegan Kevin Finisterre Richard Flay Darren Furniss +Oliver Gaste Cliff Geerdes Frederic Goddeeris Daniel Godin @@ -43,6 +47,7 @@ Mark James Michael Johnston Ricky King Thorsten Klose +Karl Knutsson Sami Korhonen Hallvard Kristiansen Alan Krum @@ -63,6 +68,7 @@ Gary Mortimer Cathy Moss Les Newell Ken Northup +Craig Nuttall Bertrand Oresve Angus Peart John Pike @@ -93,6 +99,7 @@ Alex Sowa Pete Stapley Vova Starikh Rowan Taubitz +Jim Allen Thibodaux Andrew Thoms Vladimir Timofeev Jasper Van Loenen From 857924ff6b4008b332e9b8453694376bf81e69ee Mon Sep 17 00:00:00 2001 From: Fredrik Larsson Date: Mon, 15 Sep 2014 19:51:41 +1000 Subject: [PATCH 72/74] Damn man, missed Sam - updated Credits. If I missed you also, email me. --- CREDITS.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CREDITS.txt b/CREDITS.txt index adb5b9dd9..bc2d5e175 100644 --- a/CREDITS.txt +++ b/CREDITS.txt @@ -12,6 +12,7 @@ Pete Boehl Berkely Brown Joel Brueziere Thierry Bugeat +Samuel Brugger Glenn Campigli David Carlson Mike Carr From ece0d64c9ff49497c270e5b92f64667ff43f239f Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 15 Sep 2014 15:27:19 +0200 Subject: [PATCH 73/74] OP-1434 - Update a (old, now wrong) comment --- flight/modules/StateEstimation/filtermag.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index 732cb0c5f..4fa5afa5c 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -83,7 +83,7 @@ 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); - // magBe holds the squared magnetic vector length (expected) + // magBe holds the magnetic vector length (expected) this->magBe = vector_lengthf(this->homeLocationBe, 3); this->invMagBe = 1.0f / this->magBe; RevoCalibrationGet(&this->revoCalibration); From ef341c8a0606be638b25102b79334c751aea0367 Mon Sep 17 00:00:00 2001 From: m_thread Date: Mon, 15 Sep 2014 22:49:21 +0200 Subject: [PATCH 74/74] Added Stefan Karlsson to CREDITS.txt --- CREDITS.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CREDITS.txt b/CREDITS.txt index bc2d5e175..c5fa1be78 100644 --- a/CREDITS.txt +++ b/CREDITS.txt @@ -46,6 +46,7 @@ Patrick Huebner Ryan Hunt Mark James Michael Johnston +Stefan Karlsson Ricky King Thorsten Klose Karl Knutsson