AP_AHRS: Fix some typos
Fixed some typos found in the code.
This commit is contained in:
parent
cfeaf47239
commit
28e4f78ebb
@ -242,7 +242,7 @@ void AP_AHRS::init()
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
|
#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
|
||||||
// convert to new custom rotaton
|
// convert to new custom rotation
|
||||||
// PARAMETER_CONVERSION - Added: Nov-2021
|
// PARAMETER_CONVERSION - Added: Nov-2021
|
||||||
if (_board_orientation == ROTATION_CUSTOM_OLD) {
|
if (_board_orientation == ROTATION_CUSTOM_OLD) {
|
||||||
_board_orientation.set_and_save(ROTATION_CUSTOM_1);
|
_board_orientation.set_and_save(ROTATION_CUSTOM_1);
|
||||||
@ -590,7 +590,7 @@ void AP_AHRS::update_EKF2(void)
|
|||||||
float &abias = state.accel_bias.z;
|
float &abias = state.accel_bias.z;
|
||||||
EKF2.getAccelZBias(abias);
|
EKF2.getAccelZBias(abias);
|
||||||
|
|
||||||
// This EKF is currently using primary_imu, and abias applies to only that IMU
|
// This EKF is currently using primary_imu, and a bias applies to only that IMU
|
||||||
Vector3f accel = _ins.get_accel(primary_accel);
|
Vector3f accel = _ins.get_accel(primary_accel);
|
||||||
accel.z -= abias;
|
accel.z -= abias;
|
||||||
state.accel_ef = state.dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
|
state.accel_ef = state.dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
|
||||||
@ -653,7 +653,7 @@ void AP_AHRS::update_EKF3(void)
|
|||||||
// use the same IMU as the primary EKF and correct for gyro drift
|
// use the same IMU as the primary EKF and correct for gyro drift
|
||||||
state.gyro_estimate = _ins.get_gyro(primary_gyro) + state.gyro_drift;
|
state.gyro_estimate = _ins.get_gyro(primary_gyro) + state.gyro_drift;
|
||||||
|
|
||||||
// get 3-axis accel bias festimates for active EKF (this is usually for the primary IMU)
|
// get 3-axis accel bias estimates for active EKF (this is usually for the primary IMU)
|
||||||
Vector3f &abias = state.accel_bias;
|
Vector3f &abias = state.accel_bias;
|
||||||
EKF3.getAccelBias(-1,abias);
|
EKF3.getAccelBias(-1,abias);
|
||||||
|
|
||||||
@ -821,7 +821,7 @@ bool AP_AHRS::airspeed_sensor_enabled(void) const
|
|||||||
// special case for when backend is rejecting airspeed data in
|
// special case for when backend is rejecting airspeed data in
|
||||||
// an armed fly_forward state and not dead reckoning. Then the
|
// an armed fly_forward state and not dead reckoning. Then the
|
||||||
// airspeed data is highly suspect and will be rejected. We
|
// airspeed data is highly suspect and will be rejected. We
|
||||||
// will use the synthentic airspeed instead
|
// will use the synthetic airspeed instead
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
@ -1507,7 +1507,7 @@ 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.
|
// 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.
|
// 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::get_vert_pos_rate_D(float &velocity) const
|
bool AP_AHRS::get_vert_pos_rate_D(float &velocity) const
|
||||||
{
|
{
|
||||||
switch (active_EKF_type()) {
|
switch (active_EKF_type()) {
|
||||||
@ -2118,7 +2118,7 @@ bool AP_AHRS::pre_arm_check(bool requires_position, char *failure_msg, uint8_t f
|
|||||||
|
|
||||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||||
// Always check external AHRS if enabled
|
// Always check external AHRS if enabled
|
||||||
// it is a source for IMU data even if not being used as direct AHRS replacment
|
// it is a source for IMU data even if not being used as direct AHRS replacement
|
||||||
if (AP::externalAHRS().enabled() || (ekf_type() == EKFType::EXTERNAL)) {
|
if (AP::externalAHRS().enabled() || (ekf_type() == EKFType::EXTERNAL)) {
|
||||||
if (!AP::externalAHRS().pre_arm_check(failure_msg, failure_msg_len)) {
|
if (!AP::externalAHRS().pre_arm_check(failure_msg, failure_msg_len)) {
|
||||||
return false;
|
return false;
|
||||||
@ -2923,7 +2923,7 @@ void AP_AHRS::set_terrain_hgt_stable(bool stable)
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
// return the innovations for the primariy EKF
|
// return the innovations for the primarily EKF
|
||||||
// boolean false is returned if innovations are not available
|
// boolean false is returned if innovations are not available
|
||||||
bool AP_AHRS::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
|
bool AP_AHRS::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
|
||||||
{
|
{
|
||||||
@ -2987,7 +2987,7 @@ bool AP_AHRS::is_vibration_affected() const
|
|||||||
|
|
||||||
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
|
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
|
||||||
// indicates prefect consistency between the measurement and the EKF solution and a value of 1 is the maximum
|
// indicates prefect consistency between the measurement and the EKF solution and a value of 1 is the maximum
|
||||||
// inconsistency that will be accpeted by the filter
|
// inconsistency that will be accepted by the filter
|
||||||
// boolean false is returned if variances are not available
|
// boolean false is returned if variances are not available
|
||||||
bool AP_AHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
|
bool AP_AHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
|
||||||
{
|
{
|
||||||
|
@ -271,7 +271,7 @@ AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
|
|||||||
// additional error buildup.
|
// additional error buildup.
|
||||||
|
|
||||||
// Note that we can get significant renormalisation values
|
// Note that we can get significant renormalisation values
|
||||||
// when we have a larger delta_t due to a glitch eleswhere in
|
// when we have a larger delta_t due to a glitch elsewhere in
|
||||||
// APM, such as a I2c timeout or a set of EEPROM writes. While
|
// APM, such as a I2c timeout or a set of EEPROM writes. While
|
||||||
// we would like to avoid these if possible, if it does happen
|
// we would like to avoid these if possible, if it does happen
|
||||||
// we don't want to compound the error by making DCM less
|
// we don't want to compound the error by making DCM less
|
||||||
@ -428,7 +428,7 @@ bool AP_AHRS_DCM::use_compass(void)
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (!AP::ahrs().get_fly_forward() || !have_gps()) {
|
if (!AP::ahrs().get_fly_forward() || !have_gps()) {
|
||||||
// we don't have any alterative to the compass
|
// we don't have any alternative to the compass
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
if (AP::gps().ground_speed() < GPS_SPEED_MIN) {
|
if (AP::gps().ground_speed() < GPS_SPEED_MIN) {
|
||||||
@ -463,7 +463,7 @@ bool AP_AHRS_DCM::get_quaternion(Quaternion &quat) const
|
|||||||
}
|
}
|
||||||
|
|
||||||
// yaw drift correction using the compass or GPS
|
// yaw drift correction using the compass or GPS
|
||||||
// this function prodoces the _omega_yaw_P vector, and also
|
// this function produces the _omega_yaw_P vector, and also
|
||||||
// contributes to the _omega_I.z long term yaw drift estimate
|
// contributes to the _omega_I.z long term yaw drift estimate
|
||||||
void
|
void
|
||||||
AP_AHRS_DCM::drift_correction_yaw(void)
|
AP_AHRS_DCM::drift_correction_yaw(void)
|
||||||
@ -573,8 +573,8 @@ AP_AHRS_DCM::drift_correction_yaw(void)
|
|||||||
// that depends on the spin rate. See the fastRotations.pdf
|
// that depends on the spin rate. See the fastRotations.pdf
|
||||||
// paper from Bill Premerlani
|
// paper from Bill Premerlani
|
||||||
// We also adjust the gain depending on the rate of change of horizontal velocity which
|
// We also adjust the gain depending on the rate of change of horizontal velocity which
|
||||||
// is proportional to how observable the heading is from the acceerations and GPS velocity
|
// is proportional to how observable the heading is from the accelerations and GPS velocity
|
||||||
// The accelration derived heading will be more reliable in turns than compass or GPS
|
// The acceleration derived heading will be more reliable in turns than compass or GPS
|
||||||
|
|
||||||
_omega_yaw_P.z = error_z * _P_gain(spin_rate) * _kp_yaw * _yaw_gain();
|
_omega_yaw_P.z = error_z * _P_gain(spin_rate) * _kp_yaw * _yaw_gain();
|
||||||
if (use_fast_gains()) {
|
if (use_fast_gains()) {
|
||||||
@ -1061,7 +1061,7 @@ bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const
|
|||||||
bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const
|
bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const
|
||||||
{
|
{
|
||||||
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
|
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
|
||||||
// airspeed as filled-in by an enabled airsped sensor
|
// airspeed as filled-in by an enabled airspeed sensor
|
||||||
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
|
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
|
||||||
// Or if none of the above, fills-in using the previous airspeed estimate
|
// Or if none of the above, fills-in using the previous airspeed estimate
|
||||||
// Return false: if we are using the previous airspeed estimate
|
// Return false: if we are using the previous airspeed estimate
|
||||||
@ -1085,7 +1085,7 @@ bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
|
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
|
||||||
// airspeed as filled-in by an enabled airsped sensor
|
// airspeed as filled-in by an enabled airspeed sensor
|
||||||
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
|
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
|
||||||
// Or if none of the above, fills-in using the previous airspeed estimate
|
// Or if none of the above, fills-in using the previous airspeed estimate
|
||||||
// Return false: if we are using the previous airspeed estimate
|
// Return false: if we are using the previous airspeed estimate
|
||||||
|
@ -172,7 +172,7 @@ private:
|
|||||||
void reset(bool recover_eulers);
|
void reset(bool recover_eulers);
|
||||||
|
|
||||||
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
|
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
|
||||||
// airspeed as filled-in by an enabled airsped sensor
|
// airspeed as filled-in by an enabled airspeed sensor
|
||||||
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
|
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
|
||||||
// Or if none of the above, fills-in using the previous airspeed estimate
|
// Or if none of the above, fills-in using the previous airspeed estimate
|
||||||
// Return false: if we are using the previous airspeed estimate
|
// Return false: if we are using the previous airspeed estimate
|
||||||
|
Loading…
Reference in New Issue
Block a user