AP_NavEKF2: Rename files and re-distribute content

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -223,4 +223,46 @@ bool NavEKF2_core::assume_zero_sideslip(void) const
     return _ahrs->get_fly_forward() && _ahrs->get_vehicle_class() != AHRS_VEHICLE_GROUND;
 }
 
-#endif // HAL_CPU_CLASS
+// set the LLH location of the filters NED origin
+bool NavEKF2_core::setOriginLLH(struct Location &loc)
+{
+    if (isAiding) {
+        return false;
+    }
+    EKF_origin = loc;
+    validOrigin = true;
+    return true;
+}
+
+// Set the NED origin to be used until the next filter reset
+void NavEKF2_core::setOrigin()
+{
+    // assume origin at current GPS location (no averaging)
+    EKF_origin = _ahrs->get_gps().location();
+    // define Earth rotation vector in the NED navigation frame at the origin
+    calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
+    validOrigin = true;
+    hal.console->printf("EKF Origin Set\n");
+}
+
+// Commands the EKF to not use GPS.
+// This command must be sent prior to arming
+// This command is forgotten by the EKF each time the vehicle disarms
+// Returns 0 if command rejected
+// Returns 1 if attitude, vertical velocity and vertical position will be provided
+// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
+uint8_t NavEKF2_core::setInhibitGPS(void)
+{
+    if(!isAiding) {
+        return 0;
+    }
+    if (optFlowDataPresent()) {
+        frontend._fusionModeGPS = 3;
+//#error writing to a tuning parameter
+        return 2;
+    } else {
+        return 1;
+    }
+}
+
+#endif // HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -532,41 +532,6 @@ bool NavEKF2_core::RecallGPS()
 }
 
 
-// return the Euler roll, pitch and yaw angle in radians
-void NavEKF2_core::getEulerAngles(Vector3f &euler) const
-{
-    outputDataNew.quat.to_euler(euler.x, euler.y, euler.z);
-    euler = euler - _ahrs->get_trim();
-}
-
-// return body axis gyro bias estimates in rad/sec
-void NavEKF2_core::getGyroBias(Vector3f &gyroBias) const
-{
-    if (dtIMUavg < 1e-6f) {
-        gyroBias.zero();
-        return;
-    }
-    gyroBias = stateStruct.gyro_bias / dtIMUavg;
-}
-
-// return body axis gyro scale factor error as a percentage
-void NavEKF2_core::getGyroScaleErrorPercentage(Vector3f &gyroScale) const
-{
-    if (!statesInitialised) {
-        gyroScale.x = gyroScale.y = gyroScale.z = 0;
-        return;
-    }
-    gyroScale.x = 100.0f/stateStruct.gyro_scale.x - 100.0f;
-    gyroScale.y = 100.0f/stateStruct.gyro_scale.y - 100.0f;
-    gyroScale.z = 100.0f/stateStruct.gyro_scale.z - 100.0f;
-}
-
-// return tilt error convergence metric
-void NavEKF2_core::getTiltError(float &ang) const
-{
-    ang = tiltErrFilt;
-}
-
 bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
     const AP_InertialSensor &ins = _ahrs->get_ins();
 
@@ -578,6 +543,34 @@ bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
 }
 
 
+// decay GPS horizontal position offset to close to zero at a rate of 1 m/s for copters and 5 m/s for planes
+// limit radius to a maximum of 50m
+void NavEKF2_core::decayGpsOffset()
+{
+    float offsetDecaySpd;
+    if (assume_zero_sideslip()) {
+        offsetDecaySpd = 5.0f;
+    } else {
+        offsetDecaySpd = 1.0f;
+    }
+    float lapsedTime = 0.001f*float(imuSampleTime_ms - lastDecayTime_ms);
+    lastDecayTime_ms = imuSampleTime_ms;
+    float offsetRadius = pythagorous2(gpsPosGlitchOffsetNE.x, gpsPosGlitchOffsetNE.y);
+    // decay radius if larger than offset decay speed multiplied by lapsed time (plus a margin to prevent divide by zero)
+    if (offsetRadius > (offsetDecaySpd * lapsedTime + 0.1f)) {
+        // Calculate the GPS velocity offset required. This is necessary to prevent the position measurement being rejected for inconsistency when the radius is being pulled back in.
+        gpsVelGlitchOffset = -gpsPosGlitchOffsetNE*offsetDecaySpd/offsetRadius;
+        // calculate scale factor to be applied to both offset components
+        float scaleFactor = constrain_float((offsetRadius - offsetDecaySpd * lapsedTime), 0.0f, 50.0f) / offsetRadius;
+        gpsPosGlitchOffsetNE.x *= scaleFactor;
+        gpsPosGlitchOffsetNE.y *= scaleFactor;
+    } else {
+        gpsVelGlitchOffset.zero();
+        gpsPosGlitchOffsetNE.zero();
+    }
+}
+
+
 /********************************************************
 *                  Height Measurements                  *
 ********************************************************/
@@ -700,4 +693,4 @@ void NavEKF2_core::readAirSpdData()
     }
 }
 
-#endif // HAL_CPU_CLASS
+#endif // HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp

@@ -18,6 +18,14 @@
 
 extern const AP_HAL::HAL& hal;
 
+/********************************************************
+*                   RESET FUNCTIONS                     *
+********************************************************/
+
+/********************************************************
+*                   FUSE MEASURED_DATA                  *
+********************************************************/
+
 // select fusion of optical flow measurements
 void NavEKF2_core::SelectFlowFusion()
 {
@@ -687,5 +695,8 @@ void NavEKF2_core::FuseOptFlow()
     }
 }
 
+/********************************************************
+*                   MISC FUNCTIONS                      *
+********************************************************/
 
-#endif // HAL_CPU_CLASS
+#endif // HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -76,6 +76,41 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const
 }
 
 
+// return the Euler roll, pitch and yaw angle in radians
+void NavEKF2_core::getEulerAngles(Vector3f &euler) const
+{
+    outputDataNew.quat.to_euler(euler.x, euler.y, euler.z);
+    euler = euler - _ahrs->get_trim();
+}
+
+// return body axis gyro bias estimates in rad/sec
+void NavEKF2_core::getGyroBias(Vector3f &gyroBias) const
+{
+    if (dtIMUavg < 1e-6f) {
+        gyroBias.zero();
+        return;
+    }
+    gyroBias = stateStruct.gyro_bias / dtIMUavg;
+}
+
+// return body axis gyro scale factor error as a percentage
+void NavEKF2_core::getGyroScaleErrorPercentage(Vector3f &gyroScale) const
+{
+    if (!statesInitialised) {
+        gyroScale.x = gyroScale.y = gyroScale.z = 0;
+        return;
+    }
+    gyroScale.x = 100.0f/stateStruct.gyro_scale.x - 100.0f;
+    gyroScale.y = 100.0f/stateStruct.gyro_scale.y - 100.0f;
+    gyroScale.z = 100.0f/stateStruct.gyro_scale.z - 100.0f;
+}
+
+// return tilt error convergence metric
+void NavEKF2_core::getTiltError(float &ang) const
+{
+    ang = tiltErrFilt;
+}
+
 // return the transformation matrix from XYZ (body) to NED axes
 void NavEKF2_core::getRotationBodyToNED(Matrix3f &mat) const
 {
@@ -228,6 +263,31 @@ bool NavEKF2_core::getLLH(struct Location &loc) const
 }
 
 
+// return the horizontal speed limit in m/s set by optical flow sensor limits
+// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
+void NavEKF2_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
+{
+    if (PV_AidingMode == AID_RELATIVE) {
+        // allow 1.0 rad/sec margin for angular motion
+        ekfGndSpdLimit = max((frontend._maxFlowRate - 1.0f), 0.0f) * max((terrainState - stateStruct.position[2]), rngOnGnd);
+        // use standard gains up to 5.0 metres height and reduce above that
+        ekfNavVelGainScaler = 4.0f / max((terrainState - stateStruct.position[2]),4.0f);
+    } else {
+        ekfGndSpdLimit = 400.0f; //return 80% of max filter speed
+        ekfNavVelGainScaler = 1.0f;
+    }
+}
+
+
+// return the LLH location of the filters NED origin
+bool NavEKF2_core::getOriginLLH(struct Location &loc) const
+{
+    if (validOrigin) {
+        loc = EKF_origin;
+    }
+    return validOrigin;
+}
+
 // return earth magnetic field estimates in measurement units / 1000
 void NavEKF2_core::getMagNED(Vector3f &magNED) const
 {
@@ -410,4 +470,4 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan)
 
 }
 
-#endif // HAL_CPU_CLASS
+#endif // HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -104,84 +104,6 @@ bool NavEKF2_core::resetHeightDatum(void)
     return true;
 }
 
-
-
-
-/********************************************************
-*                 GET PARAMS FUNCTIONS                  *
-********************************************************/
-
-// return the horizontal speed limit in m/s set by optical flow sensor limits
-// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
-void NavEKF2_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
-{
-    if (PV_AidingMode == AID_RELATIVE) {
-        // allow 1.0 rad/sec margin for angular motion
-        ekfGndSpdLimit = max((frontend._maxFlowRate - 1.0f), 0.0f) * max((terrainState - stateStruct.position[2]), rngOnGnd);
-        // use standard gains up to 5.0 metres height and reduce above that
-        ekfNavVelGainScaler = 4.0f / max((terrainState - stateStruct.position[2]),4.0f);
-    } else {
-        ekfGndSpdLimit = 400.0f; //return 80% of max filter speed
-        ekfNavVelGainScaler = 1.0f;
-    }
-}
-
-
-// return the LLH location of the filters NED origin
-bool NavEKF2_core::getOriginLLH(struct Location &loc) const
-{
-    if (validOrigin) {
-        loc = EKF_origin;
-    }
-    return validOrigin;
-}
-
-/********************************************************
-*            SET STATES/PARAMS FUNCTIONS                *
-********************************************************/
-
-// set the LLH location of the filters NED origin
-bool NavEKF2_core::setOriginLLH(struct Location &loc)
-{
-    if (isAiding) {
-        return false;
-    }
-    EKF_origin = loc;
-    validOrigin = true;
-    return true;
-}
-
-// Set the NED origin to be used until the next filter reset
-void NavEKF2_core::setOrigin()
-{
-    // assume origin at current GPS location (no averaging)
-    EKF_origin = _ahrs->get_gps().location();
-    // define Earth rotation vector in the NED navigation frame at the origin
-    calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
-    validOrigin = true;
-    hal.console->printf("EKF Origin Set\n");
-}
-
-// Commands the EKF to not use GPS.
-// This command must be sent prior to arming
-// This command is forgotten by the EKF each time the vehicle disarms
-// Returns 0 if command rejected
-// Returns 1 if attitude, vertical velocity and vertical position will be provided
-// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
-uint8_t NavEKF2_core::setInhibitGPS(void)
-{
-    if(!isAiding) {
-        return 0;
-    }
-    if (optFlowDataPresent()) {
-        frontend._fusionModeGPS = 3;
-//#error writing to a tuning parameter
-        return 2;
-    } else {
-        return 1;
-    }
-}
-
 /********************************************************
 *                   FUSE MEASURED_DATA                  *
 ********************************************************/
@@ -570,32 +492,4 @@ void NavEKF2_core::FuseVelPosNED()
 *                   MISC FUNCTIONS                      *
 ********************************************************/
 
-// decay GPS horizontal position offset to close to zero at a rate of 1 m/s for copters and 5 m/s for planes
+#endif // HAL_CPU_CLASS
-// limit radius to a maximum of 50m
-void NavEKF2_core::decayGpsOffset()
-{
-    float offsetDecaySpd;
-    if (assume_zero_sideslip()) {
-        offsetDecaySpd = 5.0f;
-    } else {
-        offsetDecaySpd = 1.0f;
-    }
-    float lapsedTime = 0.001f*float(imuSampleTime_ms - lastDecayTime_ms);
-    lastDecayTime_ms = imuSampleTime_ms;
-    float offsetRadius = pythagorous2(gpsPosGlitchOffsetNE.x, gpsPosGlitchOffsetNE.y);
-    // decay radius if larger than offset decay speed multiplied by lapsed time (plus a margin to prevent divide by zero)
-    if (offsetRadius > (offsetDecaySpd * lapsedTime + 0.1f)) {
-        // Calculate the GPS velocity offset required. This is necessary to prevent the position measurement being rejected for inconsistency when the radius is being pulled back in.
-        gpsVelGlitchOffset = -gpsPosGlitchOffsetNE*offsetDecaySpd/offsetRadius;
-        // calculate scale factor to be applied to both offset components
-        float scaleFactor = constrain_float((offsetRadius - offsetDecaySpd * lapsedTime), 0.0f, 50.0f) / offsetRadius;
-        gpsPosGlitchOffsetNE.x *= scaleFactor;
-        gpsPosGlitchOffsetNE.y *= scaleFactor;
-    } else {
-        gpsVelGlitchOffset.zero();
-        gpsPosGlitchOffsetNE.zero();
-    }
-}
-
-
-#endif // HAL_CPU_CLASS