aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: rename yaw_initialised access method to dcm_yaw_initialised

Browse Source 
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_AHRS/AP_AHRS.h
View File

@ -200,6 +200,11 @@ public:
// true if the AHRS has completed initialisation
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
bool get_filter_status(nav_filter_status &status) const;

6
libraries/AP_AHRS/AP_AHRS_Backend.h
View File

@ -273,11 +273,6 @@ public:
// return true if we will use compass for yaw
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
float cos_roll() const {
return _cos_roll;
@ -479,7 +474,6 @@ protected:
// flags structure
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
} _flags;

8
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -493,11 +493,11 @@ AP_AHRS_DCM::drift_correction_yaw(void)
// we force an additional compass read()
// here. This has the effect of throwing away
// 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);
_dcm_matrix.from_euler(roll, pitch, heading);
_omega_yaw_P.zero();
_flags.have_initial_yaw = true;
have_initial_yaw = true;
}
new_value = true;
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
release the GPS heading changes very rapidly
*/
if (!_flags.have_initial_yaw ||
if (!have_initial_yaw ||
yaw_deltat > 20 ||
(_gps.ground_speed() >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) {
// reset DCM matrix based on current yaw
_dcm_matrix.from_euler(roll, pitch, gps_course_rad);
_omega_yaw_P.zero();
_flags.have_initial_yaw = true;
have_initial_yaw = true;
yaw_error = 0;
}
}

7
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -62,6 +62,11 @@ public:
void update(bool skip_ins_update=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
virtual bool get_position(struct Location &loc) const override;
@ -146,6 +151,8 @@ private:
float _omega_I_sum_time;
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
Vector3f ra_delayed(uint8_t instance, const Vector3f &ra);
Vector3f _ra_delay_buffer[INS_MAX_INSTANCES];