mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-23 08:13:56 -04:00
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:
parent
60e0f47918
commit
8daa0a099c
@ -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;
|
||||||
|
|
||||||
|
@ -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;
|
||||||
|
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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];
|
||||||
|
Loading…
Reference in New Issue
Block a user