From 7b35500e9ef7660d712f52e6e0d0215a0f3c04fc Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 23 Feb 2023 11:37:52 +1300
Subject: [PATCH] offboard: two actuator control fixes

1. We no longer need to send a message immadiately as add_call_every now
   does that for us.
2. We need to make sure the vectors are resized appropriately before
   accessing them, otherwise this can trigger segfaults.

Signed-off-by: Julian Oes <julian@oes.ch>
---
 src/mavsdk/plugins/offboard/offboard_impl.cpp | 31 +++++++++----------
 1 file changed, 14 insertions(+), 17 deletions(-)

diff --git a/src/mavsdk/plugins/offboard/offboard_impl.cpp b/src/mavsdk/plugins/offboard/offboard_impl.cpp
index 92701a189a..d64ddd65f5 100644
--- a/src/mavsdk/plugins/offboard/offboard_impl.cpp
+++ b/src/mavsdk/plugins/offboard/offboard_impl.cpp
@@ -372,8 +372,8 @@ Offboard::Result OffboardImpl::set_actuator_control(Offboard::ActuatorControl ac
         }
     }
 
-    // also send it right now to reduce latency
-    return send_actuator_control();
+    // It gets sent immediately as part of add_call_every.
+    return Offboard::Result::Success;
 }
 
 Offboard::Result OffboardImpl::send_position_ned()
@@ -731,26 +731,23 @@ OffboardImpl::send_actuator_control_message(const float* controls, uint8_t group
 
 Offboard::Result OffboardImpl::send_actuator_control()
 {
-    Offboard::ActuatorControl actuator_control = [this]() {
-        std::lock_guard<std::mutex> lock(_mutex);
-        return _actuator_control;
-    }();
+    std::lock_guard<std::mutex> lock(_mutex);
 
-    for (int i = 0; i < 2; i++) {
-        int nan_count = 0;
-        for (int j = 0; j < 8; j++) {
-            if (std::isnan(actuator_control.groups[i].controls[j])) {
-                nan_count++;
-                actuator_control.groups[i].controls[j] = 0.0f;
+    for (unsigned i = 0; i < 4 && i < _actuator_control.groups.size(); ++i) {
+        _actuator_control.groups[i].controls.resize(8, NAN);
+
+        for (unsigned j = 0; j < 8; ++j) {
+            if (std::isnan(_actuator_control.groups[i].controls[j])) {
+                _actuator_control.groups[i].controls[j] = 0.0f;
             }
         }
-        if (nan_count < 8) {
-            auto result = send_actuator_control_message(&actuator_control.groups[i].controls[0], i);
-            if (result != Offboard::Result::Success) {
-                return result;
-            }
+        auto result = send_actuator_control_message(&_actuator_control.groups[i].controls[0], i);
+
+        if (result != Offboard::Result::Success) {
+            return result;
         }
     }
+
     return Offboard::Result::Success;
 }