Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_OSD: take ahrs and baro semaphores

Browse Source 
this ensures OSD data is self-consistent within each item
This commit is contained in:
Andrew Tridgell 2019-04-26 08:43:54 +10:00
parent 5023b51679
commit 48ac028cd0
1 changed files with 23 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_OSD/AP_OSD_Screen.cpp
View File

@ -427,7 +427,9 @@ float AP_OSD_Screen::u_scale(enum unit_type unit, float value)
void AP_OSD_Screen::draw_altitude(uint8_t x, uint8_t y)
{
float alt;
AP::ahrs().get_relative_position_D_home(alt);
AP_AHRS &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
ahrs.get_relative_position_D_home(alt);
alt = -alt;
backend->write(x, y, false, "%4d%c", (int)u_scale(ALTITUDE, alt), u_icon(ALTITUDE));
}
@ -556,6 +558,7 @@ void AP_OSD_Screen::draw_speed_vector(uint8_t x, uint8_t y,Vector2f v, int32_t y
void AP_OSD_Screen::draw_gspeed(uint8_t x, uint8_t y)
{
AP_AHRS &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
Vector2f v = ahrs.groundspeed_vector();
backend->write(x, y, false, "%c", SYM_GSPD);
draw_speed_vector(x + 1, y, v, ahrs.yaw_sensor);
@ -565,6 +568,7 @@ void AP_OSD_Screen::draw_gspeed(uint8_t x, uint8_t y)
void AP_OSD_Screen::draw_horizon(uint8_t x, uint8_t y)
{
AP_AHRS &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
float roll = ahrs.roll;
float pitch = -ahrs.pitch;
@ -627,6 +631,7 @@ void AP_OSD_Screen::draw_distance(uint8_t x, uint8_t y, float distance)
void AP_OSD_Screen::draw_home(uint8_t x, uint8_t y)
{
AP_AHRS &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
Location loc;
if (ahrs.get_position(loc) && ahrs.home_is_set()) {
const Location &home_loc = ahrs.get_home();
@ -693,6 +698,7 @@ void AP_OSD_Screen::draw_compass(uint8_t x, uint8_t y)
void AP_OSD_Screen::draw_wind(uint8_t x, uint8_t y)
{
AP_AHRS &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
Vector3f v = ahrs.wind_estimate();
if (check_option(AP_OSD::OPTION_INVERTED_WIND)) {
v = -v;
@ -704,7 +710,9 @@ void AP_OSD_Screen::draw_wind(uint8_t x, uint8_t y)
void AP_OSD_Screen::draw_aspeed(uint8_t x, uint8_t y)
{
float aspd = 0.0f;
bool have_estimate = AP::ahrs().airspeed_estimate(&aspd);
AP_AHRS &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
bool have_estimate = ahrs.airspeed_estimate(&aspd);
if (have_estimate) {
backend->write(x, y, false, "%c%4d%c", SYM_ASPD, (int)u_scale(SPEED, aspd), u_icon(SPEED));
} else {
@ -716,10 +724,14 @@ void AP_OSD_Screen::draw_vspeed(uint8_t x, uint8_t y)
{
Vector3f v;
float vspd;
if (AP::ahrs().get_velocity_NED(v)) {
AP_AHRS &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
if (ahrs.get_velocity_NED(v)) {
vspd = -v.z;
} else {
vspd = AP::baro().get_climb_rate();
auto &baro = AP::baro();
WITH_SEMAPHORE(baro.get_semaphore());
vspd = baro.get_climb_rate();
}
char sym;
if (vspd > 3.0f) {
@ -904,6 +916,7 @@ void AP_OSD_Screen::draw_eff(uint8_t x, uint8_t y)
{
AP_BattMonitor &battery = AP::battery();
AP_AHRS &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
Vector2f v = ahrs.groundspeed_vector();
float speed = u_scale(SPEED,v.length());
if (speed > 2.0){
@ -918,10 +931,14 @@ void AP_OSD_Screen::draw_climbeff(uint8_t x, uint8_t y)
char unit_icon = u_icon(DISTANCE);
Vector3f v;
float vspd;
if (AP::ahrs().get_velocity_NED(v)) {
auto &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
if (ahrs.get_velocity_NED(v)) {
vspd = -v.z;
} else {
vspd = AP::baro().get_climb_rate();
auto &baro = AP::baro();
WITH_SEMAPHORE(baro.get_semaphore());
vspd = baro.get_climb_rate();
}
if (vspd < 0.0) vspd = 0.0;
AP_BattMonitor &battery = AP::battery();