From 51a4b16af5c4d1c8708acffc280bfd5910194999 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 30 Jun 2013 18:49:29 +0200 Subject: [PATCH] New Filter to calculate NED from LLA as part of StateEstimation --- flight/libraries/CoordinateConversions.c | 62 +++++---- flight/libraries/inc/CoordinateConversions.h | 8 +- flight/modules/GPS/GPS.c | 22 +-- flight/modules/StateEstimation/filterlla.c | 125 ++++++++++++++++++ .../StateEstimation/inc/stateestimation.h | 2 + .../modules/StateEstimation/stateestimation.c | 54 +++++--- .../boards/coptercontrol/firmware/Makefile | 1 + flight/targets/boards/osd/firmware/Makefile | 1 + .../boards/revolution/firmware/UAVObjects.inc | 2 +- .../boards/revoproto/firmware/UAVObjects.inc | 2 +- .../boards/simposix/firmware/UAVObjects.inc | 2 +- .../src/plugins/uavobjects/uavobjects.pro | 4 +- shared/uavobjectdefinition/positionsensor.xml | 12 -- 13 files changed, 217 insertions(+), 80 deletions(-) create mode 100644 flight/modules/StateEstimation/filterlla.c delete mode 100644 shared/uavobjectdefinition/positionsensor.xml diff --git a/flight/libraries/CoordinateConversions.c b/flight/libraries/CoordinateConversions.c index 1b0a51277..fc4c66276 100644 --- a/flight/libraries/CoordinateConversions.c +++ b/flight/libraries/CoordinateConversions.c @@ -35,23 +35,29 @@ #define MIN_ALLOWABLE_MAGNITUDE 1e-30f // ****** convert Lat,Lon,Alt to ECEF ************ -void LLA2ECEF(float LLA[3], double ECEF[3]) +void LLA2ECEF(int32_t LLAi[3], double ECEF[3]) { - const double a = 6378137.0d; // Equatorial Radius - const double e = 8.1819190842622e-2d; // Eccentricity + const double a = 6378137.0d; // Equatorial Radius + const double e = 8.1819190842622e-2d; // Eccentricity + const double e2 = e * e; // Eccentricity squared double sinLat, sinLon, cosLat, cosLon; double N; + double LLA[3] = { + (double)LLAi[0] * 1e-7d, + (double)LLAi[1] * 1e-7d, + (double)LLAi[2] * 1e-4d + }; - sinLat = sin(DEG2RAD(LLA[0])); - sinLon = sin(DEG2RAD(LLA[1])); - cosLat = cos(DEG2RAD(LLA[0])); - cosLon = cos(DEG2RAD(LLA[1])); + sinLat = sin(DEG2RAD_D(LLA[0])); + sinLon = sin(DEG2RAD_D(LLA[1])); + cosLat = cos(DEG2RAD_D(LLA[0])); + cosLon = cos(DEG2RAD_D(LLA[1])); - N = a / sqrt(1.0d - e * e * sinLat * sinLat); // prime vertical radius of curvature + N = a / sqrt(1.0d - e2 * sinLat * sinLat); // prime vertical radius of curvature - ECEF[0] = (N + (double)LLA[2]) * cosLat * cosLon; - ECEF[1] = (N + (double)LLA[2]) * cosLat * sinLon; - ECEF[2] = ((1 - e * e) * N + (double)LLA[2]) * sinLat; + ECEF[0] = (N + LLA[2]) * cosLat * cosLon; + ECEF[1] = (N + LLA[2]) * cosLat * sinLon; + ECEF[2] = ((1.0d - e2) * N + LLA[2]) * sinLat; } // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* @@ -100,14 +106,14 @@ uint16_t ECEF2LLA(double ECEF[3], float LLA[3]) } // ****** find ECEF to NED rotation matrix ******** -void RneFromLLA(float LLA[3], double Rne[3][3]) +void RneFromLLA(int32_t LLAi[3], float Rne[3][3]) { - double sinLat, sinLon, cosLat, cosLon; + float sinLat, sinLon, cosLat, cosLon; - sinLat = sin(DEG2RAD(LLA[0])); - sinLon = sin(DEG2RAD(LLA[1])); - cosLat = cos(DEG2RAD(LLA[0])); - cosLon = cos(DEG2RAD(LLA[1])); + sinLat = sinf(DEG2RAD((float)LLAi[0] * 1e-7f)); + sinLon = sinf(DEG2RAD((float)LLAi[1] * 1e-7f)); + cosLat = cosf(DEG2RAD((float)LLAi[0] * 1e-7f)); + cosLon = cosf(DEG2RAD((float)LLAi[1] * 1e-7f)); Rne[0][0] = -sinLat * cosLon; Rne[0][1] = -sinLat * sinLon; @@ -188,16 +194,16 @@ void Quaternion2R(float q[4], float Rbe[3][3]) } // ****** Express LLA in a local NED Base Frame ******** -void LLA2Base(float LLA[3], double BaseECEF[3], double Rne[3][3], float NED[3]) +void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]) { double ECEF[3]; - double diff[3]; + float diff[3]; - LLA2ECEF(LLA, ECEF); + LLA2ECEF(LLAi, ECEF); - diff[0] = (ECEF[0] - BaseECEF[0]); - diff[1] = (ECEF[1] - BaseECEF[1]); - diff[2] = (ECEF[2] - BaseECEF[2]); + diff[0] = (float)(ECEF[0] - BaseECEF[0]); + diff[1] = (float)(ECEF[1] - BaseECEF[1]); + diff[2] = (float)(ECEF[2] - BaseECEF[2]); NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; @@ -205,13 +211,13 @@ void LLA2Base(float LLA[3], double BaseECEF[3], double Rne[3][3], float NED[3]) } // ****** Express ECEF in a local NED Base Frame ******** -void ECEF2Base(double ECEF[3], double BaseECEF[3], double Rne[3][3], float NED[3]) +void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3]) { - double diff[3]; + float diff[3]; - diff[0] = (ECEF[0] - BaseECEF[0]); - diff[1] = (ECEF[1] - BaseECEF[1]); - diff[2] = (ECEF[2] - BaseECEF[2]); + diff[0] = (float)(ECEF[0] - BaseECEF[0]); + diff[1] = (float)(ECEF[1] - BaseECEF[1]); + diff[2] = (float)(ECEF[2] - BaseECEF[2]); NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h index 5e25f2053..8c5f55858 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -31,12 +31,12 @@ #define COORDINATECONVERSIONS_H_ // ****** convert Lat,Lon,Alt to ECEF ************ -void LLA2ECEF(float LLA[3], double ECEF[3]); +void LLA2ECEF(int32_t LLAi[3], double ECEF[3]); // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* uint16_t ECEF2LLA(double ECEF[3], float LLA[3]); -void RneFromLLA(float LLA[3], double Rne[3][3]); +void RneFromLLA(int32_t LLAi[3], float Rne[3][3]); // ****** find rotation matrix from rotation vector void Rv2Rot(float Rv[3], float R[3][3]); @@ -51,10 +51,10 @@ void RPY2Quaternion(const float rpy[3], float q[4]); void Quaternion2R(float q[4], float Rbe[3][3]); // ****** Express LLA in a local NED Base Frame ******** -void LLA2Base(float LLA[3], double BaseECEF[3], double Rne[3][3], float NED[3]); +void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]); // ****** Express ECEF in a local NED Base Frame ******** -void ECEF2Base(double ECEF[3], double BaseECEF[3], double Rne[3][3], float NED[3]); +void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3]); // ****** convert Rotation Matrix to Quaternion ******** // ****** if R converts from e to b, q is rotation from e to b **** diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 88eccf173..5d71e0daf 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -37,7 +37,7 @@ #include "gpstime.h" #include "gpssatellites.h" #include "gpsvelocitysensor.h" -#include "systemsettings.h" +#include "gpssettings.h" #include "taskinfo.h" #include "hwsettings.h" @@ -168,13 +168,13 @@ int32_t GPSInitialize(void) #endif /* if defined(REVOLUTION) */ if (gpsPort && gpsEnabled) { - SystemSettingsInitialize(); - SystemSettingsGPSDataProtocolGet(&gpsProtocol); + GPSSettingsInitialize(); + GPSSettingsDataProtocolGet(&gpsProtocol); switch (gpsProtocol) { - case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA: + case GPSSETTINGS_DATAPROTOCOL_NMEA: gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); break; - case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX: + case GPSSETTINGS_DATAPROTOCOL_UBX: gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket)); break; default: @@ -201,9 +201,9 @@ static void gpsTask(__attribute__((unused)) void *parameters) uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; GPSPositionSensorData gpspositionsensor; - uint8_t gpsProtocol; + GPSSettingsData gpsSettings; - SystemSettingsGPSDataProtocolGet(&gpsProtocol); + GPSSettingsGet(&gpsSettings); timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; @@ -216,14 +216,14 @@ static void gpsTask(__attribute__((unused)) void *parameters) // This blocks the task until there is something on the buffer while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) { int res; - switch (gpsProtocol) { + switch (gpsSettings.DataProtocol) { #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) - case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA: + case GPSSETTINGS_DATAPROTOCOL_NMEA: res = parse_nmea_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); break; #endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) - case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX: + case GPSSETTINGS_DATAPROTOCOL_UBX: res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); break; #endif @@ -251,7 +251,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 < 3.5f) && (gpspositionsensor.Satellites >= 7) && + if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSattelites) && (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) { AlarmsClear(SYSTEMALARMS_ALARM_GPS); diff --git a/flight/modules/StateEstimation/filterlla.c b/flight/modules/StateEstimation/filterlla.c new file mode 100644 index 000000000..fa7c40339 --- /dev/null +++ b/flight/modules/StateEstimation/filterlla.c @@ -0,0 +1,125 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup State Estimation + * @brief Acquires sensor data and computes state estimate + * @{ + * + * @file filterlla.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief Computes NED position from GPS LLA data + * + * @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/stateestimation.h" +#include + +#include +#include +#include + +// Private constants + +#define STACK_REQUIRED 256 + +// Private types +struct data { + GPSSettingsData settings; + HomeLocationData home; + double HomeECEF[3]; + float HomeRne[3][3]; +}; + +// Private variables + +// Private functions + +static int32_t init(stateFilter *self); +static int32_t filter(stateFilter *self, stateEstimation *state); + + +int32_t filterLLAInitialize(stateFilter *handle) +{ + handle->init = &init; + handle->filter = &filter; + handle->localdata = pvPortMalloc(sizeof(struct data)); + GPSSettingsInitialize(); + GPSPositionSensorInitialize(); + HomeLocationInitialize(); + return STACK_REQUIRED; +} + +static int32_t init(__attribute__((unused)) stateFilter *self) +{ + struct data *this = (struct data *)self->localdata; + + GPSSettingsGet(&this->settings); + HomeLocationGet(&this->home); + if (this->home.Set != HOMELOCATION_SET_TRUE) { + // calculate home location coordinate reference + int32_t LLAi[3] = { + this->home.Latitude, + this->home.Longitude, + (int32_t)(this->home.Altitude * 1e4f), + }; + LLA2ECEF(LLAi, this->HomeECEF); + RneFromLLA(LLAi, this->HomeRne); + } + return 0; +} + +static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation *state) +{ + struct data *this = (struct data *)self->localdata; + + // cannot update local NED if home location is unset + if (this->home.Set != HOMELOCATION_SET_TRUE) { + return 0; + } + + // only do stuff if we have a valid GPS update + if (IS_SET(state->updated, SENSORUPDATES_lla)) { + // LLA information is not part of the state blob, due to its non standard layout (fixed point representation, quality of signal, ...) + // this filter deals with the gory details of interpreting it and storing it in a standard Cartesian position state + GPSPositionSensorData gpsdata; + 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) && + (gpsdata.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && + (gpsdata.Latitude != 0 || gpsdata.Longitude != 0)) { + int32_t LLAi[3] = { + gpsdata.Latitude, + gpsdata.Longitude, + (int32_t)((gpsdata.Altitude + gpsdata.GeoidSeparation) * 1e4f), + }; + LLA2Base(LLAi, this->HomeECEF, this->HomeRne, state->pos); + state->updated |= SENSORUPDATES_pos; + } + } + + return 0; +} + +/** + * @} + * @} + */ diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index 9b954874a..fe8135641 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -41,6 +41,7 @@ typedef enum { SENSORUPDATES_vel = 1 << 5, SENSORUPDATES_airspeed = 1 << 6, SENSORUPDATES_baro = 1 << 7, + SENSORUPDATES_lla = 1 << 8, } sensorUpdates; typedef struct { @@ -66,6 +67,7 @@ int32_t filterMagInitialize(stateFilter *handle); int32_t filterBaroInitialize(stateFilter *handle); int32_t filterAirInitialize(stateFilter *handle); int32_t filterStationaryInitialize(stateFilter *handle); +int32_t filterLLAInitialize(stateFilter *handle); int32_t filterCFInitialize(stateFilter *handle); int32_t filterCFMInitialize(stateFilter *handle); int32_t filterEKF13iInitialize(stateFilter *handle); diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 5ed371844..25eff0b63 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include @@ -125,6 +125,7 @@ static stateFilter magFilter; static stateFilter baroFilter; static stateFilter airFilter; static stateFilter stationaryFilter; +static stateFilter llaFilter; static stateFilter cfFilter; static stateFilter cfmFilter; static stateFilter ekf13iFilter; @@ -136,11 +137,14 @@ static filterPipeline *cfQueue = &(filterPipeline) { .next = &(filterPipeline) { .filter = &airFilter, .next = &(filterPipeline) { - .filter = &baroFilter, + .filter = &llaFilter, .next = &(filterPipeline) { - .filter = &cfFilter, - .next = NULL, - }, + .filter = &baroFilter, + .next = &(filterPipeline) { + .filter = &cfFilter, + .next = NULL, + } + } } } }; @@ -149,10 +153,13 @@ static const filterPipeline *cfmQueue = &(filterPipeline) { .next = &(filterPipeline) { .filter = &airFilter, .next = &(filterPipeline) { - .filter = &baroFilter, + .filter = &llaFilter, .next = &(filterPipeline) { - .filter = &cfmFilter, - .next = NULL, + .filter = &baroFilter, + .next = &(filterPipeline) { + .filter = &cfmFilter, + .next = NULL, + } } } } @@ -162,12 +169,15 @@ static const filterPipeline *ekf13iQueue = &(filterPipeline) { .next = &(filterPipeline) { .filter = &airFilter, .next = &(filterPipeline) { - .filter = &baroFilter, + .filter = &llaFilter, .next = &(filterPipeline) { - .filter = &stationaryFilter, + .filter = &baroFilter, .next = &(filterPipeline) { - .filter = &ekf13iFilter, - .next = NULL, + .filter = &stationaryFilter, + .next = &(filterPipeline) { + .filter = &ekf13iFilter, + .next = NULL, + } } } } @@ -178,10 +188,13 @@ static const filterPipeline *ekf13Queue = &(filterPipeline) { .next = &(filterPipeline) { .filter = &airFilter, .next = &(filterPipeline) { - .filter = &baroFilter, + .filter = &llaFilter, .next = &(filterPipeline) { - .filter = &ekf13Filter, - .next = NULL, + .filter = &baroFilter, + .next = &(filterPipeline) { + .filter = &ekf13Filter, + .next = NULL, + } } } } @@ -213,8 +226,8 @@ int32_t StateEstimationInitialize(void) MagSensorInitialize(); BaroSensorInitialize(); AirspeedSensorInitialize(); - PositionSensorInitialize(); GPSVelocitySensorInitialize(); + GPSPositionSensorInitialize(); GyroStateInitialize(); AccelStateInitialize(); @@ -230,8 +243,8 @@ int32_t StateEstimationInitialize(void) MagSensorConnectCallback(&sensorUpdatedCb); BaroSensorConnectCallback(&sensorUpdatedCb); AirspeedSensorConnectCallback(&sensorUpdatedCb); - PositionSensorConnectCallback(&sensorUpdatedCb); GPSVelocitySensorConnectCallback(&sensorUpdatedCb); + GPSPositionSensorConnectCallback(&sensorUpdatedCb); uint32_t stack_required = STACK_SIZE_BYTES; // Initialize Filters @@ -239,6 +252,7 @@ int32_t StateEstimationInitialize(void) stack_required = maxint32_t(stack_required, filterBaroInitialize(&baroFilter)); stack_required = maxint32_t(stack_required, filterAirInitialize(&airFilter)); stack_required = maxint32_t(stack_required, filterStationaryInitialize(&stationaryFilter)); + stack_required = maxint32_t(stack_required, filterLLAInitialize(&llaFilter)); stack_required = maxint32_t(stack_required, filterCFInitialize(&cfFilter)); stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter)); stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter)); @@ -353,11 +367,11 @@ static void StateEstimationCb(void) FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z); 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(PositionSensor, pos, North, East, Down); 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_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter, set to zero for now + // GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself // at this point sensor state is stored in "states" with some rudimentary filtering applied @@ -475,8 +489,8 @@ static void sensorUpdatedCb(UAVObjEvent *ev) updatedSensors |= SENSORUPDATES_mag; } - if (ev->obj == PositionSensorHandle()) { - updatedSensors |= SENSORUPDATES_pos; + if (ev->obj == GPSPositionSensorHandle()) { + updatedSensors |= SENSORUPDATES_lla; } if (ev->obj == GPSVelocitySensorHandle()) { diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index 6ca39951e..2508f6a50 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -101,6 +101,7 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/cameradesired.c SRC += $(OPUAVSYNTHDIR)/gpspositionsensor.c SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c + SRC += $(OPUAVSYNTHDIR)/gpssettings.c SRC += $(OPUAVSYNTHDIR)/hwsettings.c SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c SRC += $(OPUAVSYNTHDIR)/receiveractivity.c diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile index 9ffd1385f..70ec0624f 100644 --- a/flight/targets/boards/osd/firmware/Makefile +++ b/flight/targets/boards/osd/firmware/Makefile @@ -91,6 +91,7 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/gpssatellites.c SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c SRC += $(OPUAVSYNTHDIR)/gpstime.c + SRC += $(OPUAVSYNTHDIR)/gpssettings.c SRC += $(OPUAVSYNTHDIR)/osdsettings.c SRC += $(OPUAVSYNTHDIR)/barosensor.c SRC += $(OPUAVSYNTHDIR)/magsensor.c diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 188b15bab..0fed81339 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -49,6 +49,7 @@ UAVOBJSRCFILENAMES += gpspositionsensor UAVOBJSRCFILENAMES += gpssatellites UAVOBJSRCFILENAMES += gpstime UAVOBJSRCFILENAMES += gpsvelocitysensor +UAVOBJSRCFILENAMES += gpssettings UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus UAVOBJSRCFILENAMES += vtolpathfollowersettings @@ -65,7 +66,6 @@ UAVOBJSRCFILENAMES += overosyncsettings UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathstatus -UAVOBJSRCFILENAMES += positionsensor UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += relaytuning diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 996708520..074700e6f 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -54,6 +54,7 @@ UAVOBJSRCFILENAMES += gpspositionsensor UAVOBJSRCFILENAMES += gpssatellites UAVOBJSRCFILENAMES += gpstime UAVOBJSRCFILENAMES += gpsvelocitysensor +UAVOBJSRCFILENAMES += gpssettings UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus UAVOBJSRCFILENAMES += vtolpathfollowersettings @@ -70,7 +71,6 @@ UAVOBJSRCFILENAMES += overosyncsettings UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathstatus -UAVOBJSRCFILENAMES += positionsensor UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += relaytuning diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 58ad986ca..c48a50134 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -54,6 +54,7 @@ UAVOBJSRCFILENAMES += gpspositionsensor UAVOBJSRCFILENAMES += gpssatellites UAVOBJSRCFILENAMES += gpstime UAVOBJSRCFILENAMES += gpsvelocitysensor +UAVOBJSRCFILENAMES += gpssettings UAVOBJSRCFILENAMES += vtolpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus @@ -69,7 +70,6 @@ UAVOBJSRCFILENAMES += overosyncstats UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathstatus -UAVOBJSRCFILENAMES += positionsensor UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += revocalibration diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index a4c5c51d7..1df801ae1 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -62,11 +62,11 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/gpspositionsensor.h \ $$UAVOBJECT_SYNTHETICS/gpstime.h \ $$UAVOBJECT_SYNTHETICS/gpssatellites.h \ + $$UAVOBJECT_SYNTHETICS/gpssettings.h \ $$UAVOBJECT_SYNTHETICS/pathaction.h \ $$UAVOBJECT_SYNTHETICS/pathdesired.h \ $$UAVOBJECT_SYNTHETICS/pathstatus.h \ $$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.h \ - $$UAVOBJECT_SYNTHETICS/positionsensor.h \ $$UAVOBJECT_SYNTHETICS/positionstate.h \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \ $$UAVOBJECT_SYNTHETICS/homelocation.h \ @@ -147,11 +147,11 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/gpspositionsensor.cpp \ $$UAVOBJECT_SYNTHETICS/gpstime.cpp \ $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \ + $$UAVOBJECT_SYNTHETICS/gpssettings.cpp \ $$UAVOBJECT_SYNTHETICS/pathaction.cpp \ $$UAVOBJECT_SYNTHETICS/pathdesired.cpp \ $$UAVOBJECT_SYNTHETICS/pathstatus.cpp \ $$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.cpp \ - $$UAVOBJECT_SYNTHETICS/positionsensor.cpp \ $$UAVOBJECT_SYNTHETICS/positionstate.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \ $$UAVOBJECT_SYNTHETICS/homelocation.cpp \ diff --git a/shared/uavobjectdefinition/positionsensor.xml b/shared/uavobjectdefinition/positionsensor.xml deleted file mode 100644 index 286163871..000000000 --- a/shared/uavobjectdefinition/positionsensor.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - Contains the position in NED frame measured relative to @ref HomeLocation. - - - - - - - - -