mirror of https://github.com/ArduPilot/ardupilot
AP_WindVane: use body frame when setting apparent wind in sitl physics backend
- Change behaviour to expect apparent wind in body rather than NED frame. - Remove dependency on AP::ahrs().yaw. Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
This commit is contained in:
parent
2f1956bba0
commit
c4a86919bb
|
@ -42,8 +42,8 @@ void AP_WindVane_SITL::update_direction()
|
||||||
_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().yaw);
|
||||||
|
|
||||||
} else { // WINDVANE_SITL_APARRENT
|
} else { // WINDVANE_SITL_APARRENT
|
||||||
// directly read the apparent wind from as set by physics backend
|
// directly read the body frame apparent wind set by physics backend
|
||||||
_frontend._direction_apparent_raw = wrap_PI(AP::sitl()->get_apparent_wind_dir() - AP::ahrs().yaw);
|
_frontend._direction_apparent_raw = wrap_PI(AP::sitl()->get_apparent_wind_dir());
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue