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