From 3d466803481bd42ded6cb1f0038629085a584e3a Mon Sep 17 00:00:00 2001 From: priseborough Date: Thu, 8 Jan 2015 13:43:41 +1100 Subject: [PATCH] AP_NavEKF: Make NED origin independent of home position This enables the filter to report the last known position after disarm. The LLH location of the filters NED origin is published and should be logged every time the vehicle is armed to assist with post-flight trajectory reconstruction. The LLH location of the filters NED origin can be set externally whilst the vehicle is disarmed. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 69 ++++++++++++++++++++++++------- libraries/AP_NavEKF/AP_NavEKF.h | 16 +++++++ 2 files changed, 71 insertions(+), 14 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 4021a728f4..eb387f527d 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -3639,17 +3639,21 @@ void NavEKF::getMagXYZ(Vector3f &magXYZ) const // return the last calculated latitude, longitude and height bool NavEKF::getLLH(struct Location &loc) const { - loc.lat = _ahrs->get_home().lat; - loc.lng = _ahrs->get_home().lng; - loc.alt = _ahrs->get_home().alt - state.position.z*100; - loc.flags.relative_alt = 0; - loc.flags.terrain_alt = 0; - if (constPosMode) { - location_offset(loc, lastKnownPositionNE.x, lastKnownPositionNE.y); + if(validOrigin) { + loc.lat = EKF_origin.lat; + loc.lng = EKF_origin.lng; + loc.alt = EKF_origin.alt - state.position.z*100; + loc.flags.relative_alt = 0; + loc.flags.terrain_alt = 0; + if (constPosMode) { + location_offset(loc, lastKnownPositionNE.x, lastKnownPositionNE.y); + } else { + location_offset(loc, state.position.x, state.position.y); + } + return true; } else { - location_offset(loc, state.position.x, state.position.y); + return false; } - return true; } // return the estimated height above ground level @@ -3964,9 +3968,15 @@ void NavEKF::readGpsData() } } - // read latitutde and longitude from GPS and convert to NE position - const struct Location &gpsloc = _ahrs->get_gps().location(); - gpsPosNE = location_diff(_ahrs->get_home(), gpsloc); + // read latitutde and longitude from GPS and convert to local NE position relative to the stored origin + // If we don't have an origin, then set it to the current GPS coordinates + if (validOrigin) { + const struct Location &gpsloc = _ahrs->get_gps().location(); + gpsPosNE = location_diff(EKF_origin, gpsloc); + } else { + setOrigin(); + gpsPosNE.zero(); + } // calculate a position offset which is applied to NE position and velocity wherever it is used throughout code to allow GPS position jumps to be accommodated gradually decayGpsOffset(); } @@ -4372,6 +4382,7 @@ void NavEKF::InitialiseVariables() inhibitMagStates = true; gndOffsetValid = false; flowXfailed = false; + validOrigin = false; } // return true if we should use the airspeed sensor @@ -4548,8 +4559,11 @@ void NavEKF::performArmingChecks() } // zero stored velocities used to do dead-reckoning heldVelNE.zero(); - // zero last known position used to deadreckon if GPS is lost - lastKnownPositionNE.zero(); + // zero last known position used to deadreckon if GPS is lost when arming + // keep position during disarm to provide continuing indication of last known position + if (vehicleArmed) { + lastKnownPositionNE.zero(); + } // set various useage modes based on the condition at arming. These are then held until the vehicle is disarmed. if (!vehicleArmed) { PV_AidingMode = AID_NONE; // When dis-armed, we only estimate orientation & height using the constant position mode @@ -4598,4 +4612,31 @@ void NavEKF::performArmingChecks() } +// Set the NED origin to be used until the next filter reset +void NavEKF::setOrigin() +{ + EKF_origin = _ahrs->get_gps().location(); + validOrigin = true; +} + +// return the LLH location of the filters NED origin +bool NavEKF::getOriginLLH(struct Location &loc) const +{ + if (validOrigin) { + loc = EKF_origin; + } + return validOrigin; +} + +// set the LLH location of the filters NED origin +bool NavEKF::setOriginLLH(struct Location &loc) +{ + if (vehicleArmed) { + return false; + } + EKF_origin = loc; + validOrigin = true; + return true; +} + #endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 3d9be01221..9850d8448d 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -139,6 +139,17 @@ public: // return the last calculated latitude, longitude and height bool getLLH(struct Location &loc) const; + // return the latitude and longitude and height used to set the NED origin + // All NED positions calculated by the filter are relative to this location + // Returns false if the origin has not been set + bool getOriginLLH(struct Location &loc) const; + + // set the latitude and longitude and height used to set the NED origin + // All NED positions calcualted by the filter will be relative to this location + // The origin cannot be set if the filter is in a flight mode (eg vehicle armed) + // Returns false if the filter has rejected the attempt to set the origin + bool setOriginLLH(struct Location &loc); + // return estimated height above ground level // return false if ground height is not being estimated. bool getHAGL(float &HAGL) const; @@ -379,6 +390,9 @@ private: // Check arm status and perform required checks and mode changes void performArmingChecks(); + // Set the NED origin to be used until the next filter reset + void setOrigin(); + // EKF Mavlink Tuneable Parameters AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s @@ -572,6 +586,8 @@ private: bool gpsNotAvailable; // bool true when valid GPS data is not available bool vehicleArmed; // true when the vehicle is disarmed bool prevVehicleArmed; // vehicleArmed from previous frame + struct Location EKF_origin; // LLH origin of the NED axis system - do not change unless filter is reset + bool validOrigin; // true when the EKF origin is valid // Used by smoothing of state corrections float gpsIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement