mirror of https://github.com/ArduPilot/ardupilot
AP_AHRS: clarify get_vert_pos_rate AHRS method name to include 'D'
This commit is contained in:
parent
15682fa6c9
commit
904707de15
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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)) {
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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)
|
||||
|
|
Loading…
Reference in New Issue