From 5981d9acd67edf55e94096ea9a024d54728642c5 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 6 Nov 2012 10:13:09 +0100 Subject: [PATCH] removed surplus NEDposition uavobject --- flight/Modules/Attitude/revolution/attitude.c | 17 ------------ .../fixedwingpathfollower.c | 1 - .../VtolPathFollower/vtolpathfollower.c | 26 ++++++++++++++----- flight/RevoMini/UAVObjects.inc | 1 - flight/Revolution/UAVObjects.inc | 1 - flight/SimPosix/UAVObjects.inc | 1 - .../src/plugins/uavobjects/uavobjects.pro | 2 -- shared/uavobjectdefinition/nedposition.xml | 12 --------- 8 files changed, 20 insertions(+), 41 deletions(-) delete mode 100644 shared/uavobjectdefinition/nedposition.xml diff --git a/flight/Modules/Attitude/revolution/attitude.c b/flight/Modules/Attitude/revolution/attitude.c index 36261f94e..7fc84f1c5 100644 --- a/flight/Modules/Attitude/revolution/attitude.c +++ b/flight/Modules/Attitude/revolution/attitude.c @@ -63,7 +63,6 @@ #include "gyrosbias.h" #include "homelocation.h" #include "magnetometer.h" -#include "nedposition.h" #include "positionactual.h" #include "revocalibration.h" #include "revosettings.h" @@ -128,7 +127,6 @@ int32_t AttitudeInitialize(void) { AttitudeActualInitialize(); AttitudeSettingsInitialize(); - NEDPositionInitialize(); PositionActualInitialize(); VelocityActualInitialize(); RevoSettingsInitialize(); @@ -455,13 +453,6 @@ static int32_t updateAttitudeComplementary(bool first_run) GPSPositionGet(&gpsPosition); getNED(&gpsPosition, NED); - NEDPositionData nedPosition; - NEDPositionGet(&nedPosition); - nedPosition.North = NED[0]; - nedPosition.East = NED[1]; - nedPosition.Down = NED[2]; - NEDPositionSet(&nedPosition); - PositionActualData positionActual; PositionActualGet(&positionActual); positionActual.North = NED[0]; @@ -776,14 +767,6 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) (1.0f - BARO_OFFSET_LOWPASS_ALPHA ) * ( -NED[2] - baroData.Altitude ); - // Store this for inspecting offline - NEDPositionData nedPos; - NEDPositionGet(&nedPos); - nedPos.North = NED[0]; - nedPos.East = NED[1]; - nedPos.Down = NED[2]; - NEDPositionSet(&nedPos); - } else if (!outdoor_mode) { INSSetPosVelVar(1e2f, 1e2f); vel[0] = vel[1] = vel[2] = 0; diff --git a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c index b7d423e97..66402ca7b 100644 --- a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -60,7 +60,6 @@ #include "fixedwingpathfollowersettings.h" #include "fixedwingpathfollowerstatus.h" #include "homelocation.h" -#include "nedposition.h" #include "stabilizationdesired.h" #include "stabilizationsettings.h" #include "systemsettings.h" diff --git a/flight/Modules/VtolPathFollower/vtolpathfollower.c b/flight/Modules/VtolPathFollower/vtolpathfollower.c index 74280c4c7..3781f492f 100644 --- a/flight/Modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/Modules/VtolPathFollower/vtolpathfollower.c @@ -60,9 +60,9 @@ #include "pathstatus.h" #include "gpsvelocity.h" #include "gpsposition.h" +#include "homelocation.h" #include "vtolpathfollowersettings.h" #include "nedaccel.h" -#include "nedposition.h" #include "stabilizationdesired.h" #include "stabilizationsettings.h" #include "systemsettings.h" @@ -75,6 +75,7 @@ #define STACK_SIZE_BYTES 1548 #define TASK_PRIORITY (tskIDLE_PRIORITY+2) #define F_PI 3.14159265358979323846f +#define DEG2RAD (F_PI/180.0f) // Private types @@ -348,11 +349,24 @@ void updateEndpointVelocity() break; case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: { - NEDPositionData nedPosition; - NEDPositionGet(&nedPosition); - northPos = nedPosition.North; - eastPos = nedPosition.East; - downPos = nedPosition.Down; + // this used to work with the NEDposition UAVObject + // however this UAVObject has been removed + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); + float lat = homeLocation.Latitude / 10.0e6f * DEG2RAD; + float alt = homeLocation.Altitude; + float T[3] = { alt+6.378137E6f, + cosf(lat)*(alt+6.378137E6f), + -1.0f}; + float NED[3] = {T[0] * ((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD), + T[1] * ((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD), + T[2] * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude))}; + + northPos = NED[0]; + eastPos = NED[1]; + downPos = NED[2]; } break; default: diff --git a/flight/RevoMini/UAVObjects.inc b/flight/RevoMini/UAVObjects.inc index 6a5b8bbfb..b00cfe2ce 100644 --- a/flight/RevoMini/UAVObjects.inc +++ b/flight/RevoMini/UAVObjects.inc @@ -63,7 +63,6 @@ UAVOBJSRCFILENAMES += manualcontrolsettings UAVOBJSRCFILENAMES += mixersettings UAVOBJSRCFILENAMES += mixerstatus UAVOBJSRCFILENAMES += nedaccel -UAVOBJSRCFILENAMES += nedposition UAVOBJSRCFILENAMES += objectpersistence UAVOBJSRCFILENAMES += overosyncstats UAVOBJSRCFILENAMES += overosyncsettings diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index 4fd159609..0a8485ea8 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -63,7 +63,6 @@ UAVOBJSRCFILENAMES += manualcontrolsettings UAVOBJSRCFILENAMES += mixersettings UAVOBJSRCFILENAMES += mixerstatus UAVOBJSRCFILENAMES += nedaccel -UAVOBJSRCFILENAMES += nedposition UAVOBJSRCFILENAMES += objectpersistence UAVOBJSRCFILENAMES += overosyncstats UAVOBJSRCFILENAMES += overosyncsettings diff --git a/flight/SimPosix/UAVObjects.inc b/flight/SimPosix/UAVObjects.inc index faf6cc269..62f347f6c 100644 --- a/flight/SimPosix/UAVObjects.inc +++ b/flight/SimPosix/UAVObjects.inc @@ -61,7 +61,6 @@ UAVOBJSRCFILENAMES += manualcontrolsettings UAVOBJSRCFILENAMES += mixersettings UAVOBJSRCFILENAMES += mixerstatus UAVOBJSRCFILENAMES += nedaccel -UAVOBJSRCFILENAMES += nedposition UAVOBJSRCFILENAMES += objectpersistence UAVOBJSRCFILENAMES += overosyncstats UAVOBJSRCFILENAMES += pathaction diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 9b19dd72c..6f170c840 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -87,7 +87,6 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/flightplancontrol.h \ $$UAVOBJECT_SYNTHETICS/watchdogstatus.h \ $$UAVOBJECT_SYNTHETICS/nedaccel.h \ - $$UAVOBJECT_SYNTHETICS/nedposition.h \ $$UAVOBJECT_SYNTHETICS/sonaraltitude.h \ $$UAVOBJECT_SYNTHETICS/flightstatus.h \ $$UAVOBJECT_SYNTHETICS/hwsettings.h \ @@ -167,7 +166,6 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/flightplancontrol.cpp \ $$UAVOBJECT_SYNTHETICS/watchdogstatus.cpp \ $$UAVOBJECT_SYNTHETICS/nedaccel.cpp \ - $$UAVOBJECT_SYNTHETICS/nedposition.cpp \ $$UAVOBJECT_SYNTHETICS/sonaraltitude.cpp \ $$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp \ $$UAVOBJECT_SYNTHETICS/flightstatus.cpp \ diff --git a/shared/uavobjectdefinition/nedposition.xml b/shared/uavobjectdefinition/nedposition.xml deleted file mode 100644 index 7fc6eb315..000000000 --- a/shared/uavobjectdefinition/nedposition.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - Contains the current position relative to @ref HomeLocation - - - - - - - - -