diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 86f5bb4f9e..49b2a0d85c 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -1400,11 +1400,11 @@ bool AP_AHRS::get_mag_field_correction(Vector3f &vec) const // Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops. // This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for. -bool AP_AHRS::get_vert_pos_rate(float &velocity) const +bool AP_AHRS::get_vert_pos_rate_D(float &velocity) const { switch (active_EKF_type()) { case EKFType::NONE: - return dcm.get_vert_pos_rate(velocity); + return dcm.get_vert_pos_rate_D(velocity); #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: @@ -1420,11 +1420,11 @@ bool AP_AHRS::get_vert_pos_rate(float &velocity) const #if AP_AHRS_SIM_ENABLED case EKFType::SIM: - return sim.get_vert_pos_rate(velocity); + return sim.get_vert_pos_rate_D(velocity); #endif #if HAL_EXTERNAL_AHRS_ENABLED case EKFType::EXTERNAL: - return external.get_vert_pos_rate(velocity); + return external.get_vert_pos_rate_D(velocity); #endif } // since there is no default case above, this is unreachable diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index f3cc56f057..5fa7550e1b 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -229,7 +229,7 @@ public: // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. - bool get_vert_pos_rate(float &velocity) const; + bool get_vert_pos_rate_D(float &velocity) const; // write optical flow measurements to EKF void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, const float heightOverride); diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index baa8ef4e7d..6675a9c288 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -177,7 +177,7 @@ public: // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. - virtual bool get_vert_pos_rate(float &velocity) const = 0; + virtual bool get_vert_pos_rate_D(float &velocity) const = 0; // returns the estimated magnetic field offsets in body frame virtual bool get_mag_field_correction(Vector3f &ret) const WARN_IF_UNUSED { diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 5e053688ee..ce8b038334 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -1183,7 +1183,7 @@ bool AP_AHRS_DCM::get_velocity_NED(Vector3f &vec) const // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. -bool AP_AHRS_DCM::get_vert_pos_rate(float &velocity) const +bool AP_AHRS_DCM::get_vert_pos_rate_D(float &velocity) const { Vector3f velned; if (!get_velocity_NED(velned)) { diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 800e7b681c..168cbbaa33 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -111,7 +111,7 @@ public: // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. - bool get_vert_pos_rate(float &velocity) const override; + bool get_vert_pos_rate_D(float &velocity) const override; // returns false if we fail arming checks, in which case the buffer will be populated with a failure message // requires_position should be true if horizontal position configuration should be checked (not used) diff --git a/libraries/AP_AHRS/AP_AHRS_External.cpp b/libraries/AP_AHRS/AP_AHRS_External.cpp index 3d09b8e723..0c1a01b2c7 100644 --- a/libraries/AP_AHRS/AP_AHRS_External.cpp +++ b/libraries/AP_AHRS/AP_AHRS_External.cpp @@ -100,7 +100,7 @@ bool AP_AHRS_External::get_velocity_NED(Vector3f &vec) const return AP::externalAHRS().get_velocity_NED(vec); } -bool AP_AHRS_External::get_vert_pos_rate(float &velocity) const +bool AP_AHRS_External::get_vert_pos_rate_D(float &velocity) const { return AP::externalAHRS().get_speed_down(velocity); } diff --git a/libraries/AP_AHRS/AP_AHRS_External.h b/libraries/AP_AHRS/AP_AHRS_External.h index 5a21cebad1..999ed05fed 100644 --- a/libraries/AP_AHRS/AP_AHRS_External.h +++ b/libraries/AP_AHRS/AP_AHRS_External.h @@ -72,7 +72,7 @@ public: // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. - bool get_vert_pos_rate(float &velocity) const override; + bool get_vert_pos_rate_D(float &velocity) const override; // returns false if we fail arming checks, in which case the buffer will be populated with a failure message // requires_position should be true if horizontal position configuration should be checked (not used) diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.cpp b/libraries/AP_AHRS/AP_AHRS_SIM.cpp index 0714d15435..ed6cd01bb8 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_SIM.cpp @@ -80,7 +80,7 @@ Vector2f AP_AHRS_SIM::groundspeed_vector(void) return Vector2f(fdm.speedN, fdm.speedE); } -bool AP_AHRS_SIM::get_vert_pos_rate(float &velocity) const +bool AP_AHRS_SIM::get_vert_pos_rate_D(float &velocity) const { if (_sitl == nullptr) { return false; diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.h b/libraries/AP_AHRS/AP_AHRS_SIM.h index 3caf1051b0..340a8877b6 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.h +++ b/libraries/AP_AHRS/AP_AHRS_SIM.h @@ -92,7 +92,7 @@ public: // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. - bool get_vert_pos_rate(float &velocity) const override; + bool get_vert_pos_rate_D(float &velocity) const override; // returns false if we fail arming checks, in which case the buffer will be populated with a failure message // requires_position should be true if horizontal position configuration should be checked (not used)