AP_WindVane: make AHRS attitude member variables private

This commit is contained in:
Peter Barker 2024-01-12 23:40:23 +11:00 committed by Peter Barker
parent 1183328266
commit c4dc0ae101
3 changed files with 6 additions and 6 deletions

View File

@ -321,7 +321,7 @@ void AP_WindVane::update()
} }
} else { } else {
// only have direction, can't do true wind calcs, set true direction to apparent + heading // only have direction, can't do true wind calcs, set true direction to apparent + heading
_direction_true_raw = wrap_PI(_direction_apparent_raw + AP::ahrs().yaw); _direction_true_raw = wrap_PI(_direction_apparent_raw + AP::ahrs().get_yaw());
_speed_true_raw = 0.0f; _speed_true_raw = 0.0f;
} }
@ -395,7 +395,7 @@ void AP_WindVane::update()
void AP_WindVane::record_home_heading() void AP_WindVane::record_home_heading()
{ {
_home_heading = AP::ahrs().yaw; _home_heading = AP::ahrs().get_yaw();
} }
// to start direction calibration from mavlink or other // to start direction calibration from mavlink or other
@ -454,7 +454,7 @@ void AP_WindVane::update_true_wind_speed_and_direction()
} }
// convert apparent wind speed and direction to 2D vector in same frame as vehicle velocity // convert apparent wind speed and direction to 2D vector in same frame as vehicle velocity
const float wind_dir_180 = _direction_apparent_raw + AP::ahrs().yaw + radians(180); const float wind_dir_180 = _direction_apparent_raw + AP::ahrs().get_yaw() + radians(180);
const Vector2f wind_apparent_vec(cosf(wind_dir_180) * _speed_apparent, sinf(wind_dir_180) * _speed_apparent); const Vector2f wind_apparent_vec(cosf(wind_dir_180) * _speed_apparent, sinf(wind_dir_180) * _speed_apparent);
// add vehicle velocity // add vehicle velocity
@ -482,7 +482,7 @@ void AP_WindVane::update_apparent_wind_dir_from_true()
Vector2f wind_apparent_vec = Vector2f(wind_true_vec.x - veh_velocity.x, wind_true_vec.y - veh_velocity.y); Vector2f wind_apparent_vec = Vector2f(wind_true_vec.x - veh_velocity.x, wind_true_vec.y - veh_velocity.y);
// calculate apartment speed and direction // calculate apartment speed and direction
_direction_apparent_raw = wrap_PI(atan2f(wind_apparent_vec.y, wind_apparent_vec.x) - radians(180) - AP::ahrs().yaw); _direction_apparent_raw = wrap_PI(atan2f(wind_apparent_vec.y, wind_apparent_vec.x) - radians(180) - AP::ahrs().get_yaw());
_speed_apparent_raw = wind_apparent_vec.length(); _speed_apparent_raw = wind_apparent_vec.length();
} }

View File

@ -32,7 +32,7 @@ void AP_WindVane_Home::update_direction()
} }
} }
_frontend._direction_apparent_raw = wrap_PI(direction_apparent_ef - AP::ahrs().yaw); _frontend._direction_apparent_raw = wrap_PI(direction_apparent_ef - AP::ahrs().get_yaw());
} }
#endif // AP_WINDVANE_HOME_ENABLED #endif // AP_WINDVANE_HOME_ENABLED

View File

@ -41,7 +41,7 @@ void AP_WindVane_SITL::update_direction()
wind_vector_ef.x += AP::sitl()->state.speedN; wind_vector_ef.x += AP::sitl()->state.speedN;
wind_vector_ef.y += AP::sitl()->state.speedE; wind_vector_ef.y += AP::sitl()->state.speedE;
_frontend._direction_apparent_raw = wrap_PI(atan2f(wind_vector_ef.y, wind_vector_ef.x) - AP::ahrs().yaw); _frontend._direction_apparent_raw = wrap_PI(atan2f(wind_vector_ef.y, wind_vector_ef.x) - AP::ahrs().get_yaw());
} else { // WINDVANE_SITL_APARRENT } else { // WINDVANE_SITL_APARRENT
// directly read the body frame apparent wind set by physics backend // directly read the body frame apparent wind set by physics backend