From 48ac028cd00778a64bf4ab1e140833746a8b9571 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 26 Apr 2019 08:43:54 +1000
Subject: [PATCH] AP_OSD: take ahrs and baro semaphores

this ensures OSD data is self-consistent within each item
---
 libraries/AP_OSD/AP_OSD_Screen.cpp | 29 +++++++++++++++++++++++------
 1 file changed, 23 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp
index 58d0012344..2e7dab824a 100644
--- a/libraries/AP_OSD/AP_OSD_Screen.cpp
+++ b/libraries/AP_OSD/AP_OSD_Screen.cpp
@@ -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();