AP_AHRS: rename yaw_initialised access method to dcm_yaw_initialised

This is what it really means.

Also put the relevant state within the DCM object
This commit is contained in:
Peter Barker 2021-08-12 10:15:00 +10:00 committed by Andrew Tridgell
parent 60e0f47918
commit 8daa0a099c
4 changed files with 16 additions and 10 deletions

View File

@ -200,6 +200,11 @@ public:
// true if the AHRS has completed initialisation // true if the AHRS has completed initialisation
bool initialised() const override; bool initialised() const override;
// return true if *DCM* yaw has been initialised
bool dcm_yaw_initialised(void) const {
return AP_AHRS_DCM::yaw_initialised();
}
// get_filter_status - returns filter status as a series of flags // get_filter_status - returns filter status as a series of flags
bool get_filter_status(nav_filter_status &status) const; bool get_filter_status(nav_filter_status &status) const;

View File

@ -273,11 +273,6 @@ public:
// return true if we will use compass for yaw // return true if we will use compass for yaw
virtual bool use_compass(void) = 0; virtual bool use_compass(void) = 0;
// return true if yaw has been initialised
bool yaw_initialised(void) const {
return _flags.have_initial_yaw;
}
// helper trig value accessors // helper trig value accessors
float cos_roll() const { float cos_roll() const {
return _cos_roll; return _cos_roll;
@ -479,7 +474,6 @@ protected:
// flags structure // flags structure
struct ahrs_flags { struct ahrs_flags {
uint8_t have_initial_yaw : 1; // whether the yaw value has been intialised with a reference
uint8_t wind_estimation : 1; // 1 if we should do wind estimation uint8_t wind_estimation : 1; // 1 if we should do wind estimation
} _flags; } _flags;

View File

@ -493,11 +493,11 @@ AP_AHRS_DCM::drift_correction_yaw(void)
// we force an additional compass read() // we force an additional compass read()
// here. This has the effect of throwing away // here. This has the effect of throwing away
// the first compass value, which can be bad // the first compass value, which can be bad
if (!_flags.have_initial_yaw && compass.read()) { if (!have_initial_yaw && compass.read()) {
const float heading = compass.calculate_heading(_dcm_matrix); const float heading = compass.calculate_heading(_dcm_matrix);
_dcm_matrix.from_euler(roll, pitch, heading); _dcm_matrix.from_euler(roll, pitch, heading);
_omega_yaw_P.zero(); _omega_yaw_P.zero();
_flags.have_initial_yaw = true; have_initial_yaw = true;
} }
new_value = true; new_value = true;
yaw_error = yaw_error_compass(compass); yaw_error = yaw_error_compass(compass);
@ -536,13 +536,13 @@ AP_AHRS_DCM::drift_correction_yaw(void)
operator pulls back the plane rapidly enough then on operator pulls back the plane rapidly enough then on
release the GPS heading changes very rapidly release the GPS heading changes very rapidly
*/ */
if (!_flags.have_initial_yaw || if (!have_initial_yaw ||
yaw_deltat > 20 || yaw_deltat > 20 ||
(_gps.ground_speed() >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) { (_gps.ground_speed() >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) {
// reset DCM matrix based on current yaw // reset DCM matrix based on current yaw
_dcm_matrix.from_euler(roll, pitch, gps_course_rad); _dcm_matrix.from_euler(roll, pitch, gps_course_rad);
_omega_yaw_P.zero(); _omega_yaw_P.zero();
_flags.have_initial_yaw = true; have_initial_yaw = true;
yaw_error = 0; yaw_error = 0;
} }
} }

View File

@ -62,6 +62,11 @@ public:
void update(bool skip_ins_update=false) override; void update(bool skip_ins_update=false) override;
void reset(bool recover_eulers = false) override; void reset(bool recover_eulers = false) override;
// return true if yaw has been initialised
bool yaw_initialised(void) const {
return have_initial_yaw;
}
// dead-reckoning support // dead-reckoning support
virtual bool get_position(struct Location &loc) const override; virtual bool get_position(struct Location &loc) const override;
@ -146,6 +151,8 @@ private:
float _omega_I_sum_time; float _omega_I_sum_time;
Vector3f _omega; // Corrected Gyro_Vector data Vector3f _omega; // Corrected Gyro_Vector data
bool have_initial_yaw; // true if the yaw value has been initialised with a reference
// variables to cope with delaying the GA sum to match GPS lag // variables to cope with delaying the GA sum to match GPS lag
Vector3f ra_delayed(uint8_t instance, const Vector3f &ra); Vector3f ra_delayed(uint8_t instance, const Vector3f &ra);
Vector3f _ra_delay_buffer[INS_MAX_INSTANCES]; Vector3f _ra_delay_buffer[INS_MAX_INSTANCES];