Skip to content

AP_AHRS: combine copy_estimates_from_backend_estimates and update_state #33221

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
peterbarker wants to merge 1 commit into
base: master
Choose a base branch
from
+20 −34
Open

AP_AHRS: combine copy_estimates_from_backend_estimates and update_state #33221

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
47 changes: 20 additions & 27 deletions libraries/AP_AHRS/AP_AHRS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -438,7 +438,9 @@ void AP_AHRS::reset_gyro_drift(void)
}

/*
update state structure after each update()
* copy results from a backend over AP_AHRS canonical results.
* This updates member variables like roll and pitch, as well as
* updating derived values like sin_roll and sin_pitch.
*/
void AP_AHRS::update_state(void)
{
Expand All @@ -458,6 +460,22 @@ void AP_AHRS::update_state(void)
state.airspeed_EAS_ok = _airspeed_EAS(state.airspeed_EAS, state.airspeed_estimate_type);
state.airspeed_TAS_ok = _airspeed_TAS(state.airspeed_TAS);
state.airspeed_TAS_vec_ok = _airspeed_TAS(state.airspeed_TAS_vec);

roll = active_estimates->roll_rad;
pitch = active_estimates->pitch_rad;
yaw = active_estimates->yaw_rad;

state.dcm_matrix = active_estimates->dcm_matrix;

state.gyro_estimate = active_estimates->gyro_estimate;
state.gyro_drift = active_estimates->gyro_drift;

state.accel_ef = active_estimates->accel_ef;
state.accel_bias = active_estimates->accel_bias;

update_cd_values();
update_trig();

state.quat_ok = active_estimates->get_quaternion(state.quat);
state.secondary_attitude_ok = _get_secondary_attitude(state.secondary_attitude);
state.secondary_quat_ok = _get_secondary_quaternion(state.secondary_quat);
Expand Down Expand Up @@ -572,11 +590,9 @@ void AP_AHRS::update(bool skip_ins_update)
}
#endif // HAL_GCS_ENABLED

// update published state
// update published state, including copying state from the active backend:
update_state();

copy_estimates_from_backend_estimates(*active_estimates);

#if AP_MODULE_SUPPORTED
// call AHRS_update hook if any
AP_Module::call_hook_AHRS_update(*this);
Expand Down Expand Up @@ -607,29 +623,6 @@ void AP_AHRS::update(bool skip_ins_update)
#endif
}

/*
* copy results from a backend over AP_AHRS canonical results.
* This updates member variables like roll and pitch, as well as
* updating derived values like sin_roll and sin_pitch.
*/
void AP_AHRS::copy_estimates_from_backend_estimates(const AP_AHRS_Backend::Estimates &results)
{
roll = results.roll_rad;
pitch = results.pitch_rad;
yaw = results.yaw_rad;

state.dcm_matrix = results.dcm_matrix;

state.gyro_estimate = results.gyro_estimate;
state.gyro_drift = results.gyro_drift;

state.accel_ef = results.accel_ef;
state.accel_bias = results.accel_bias;

update_cd_values();
update_trig();
}

#if AP_AHRS_DCM_ENABLED
void AP_AHRS::update_DCM()
{
Expand Down
7 changes: 0 additions & 7 deletions libraries/AP_AHRS/AP_AHRS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -924,13 +924,6 @@ class AP_AHRS {
// poke AP_Notify based on values from status
void update_notify_from_filter_status(const nav_filter_status &status);

/*
* copy results from a backend over AP_AHRS canonical results.
* This updates member variables like roll and pitch, as well as
* updating derived values like sin_roll and sin_pitch.
*/
void copy_estimates_from_backend_estimates(const AP_AHRS_Backend::Estimates &results);

// write out secondary estimates:
void Write_AHRS2(void) const;
// write POS (canonical vehicle position) message out:
Expand Down
Loading