firmware sends offboard command for timing tests

Author: JMoore5353

comms/mavlink/mavlink.cpp

@@ -479,6 +479,17 @@ void Mavlink::handle_msg_offboard_control(const mavlink_message_t * const msg)
   control.Fz.valid = !(ctrl.ignore & IGNORE_VALUE6);
 
   if (listener_ != nullptr) { listener_->offboard_control_callback(control); }
+
+  received_offboard_ctrl_ = true;
+  prev_offboard_msg_ = *msg;
+}
+
+void Mavlink::send_previous_offboard_control()
+{
+  if (received_offboard_ctrl_) {
+    send_message(prev_offboard_msg_);
+    received_offboard_ctrl_ = false;
+  }
 }
 
 void Mavlink::handle_msg_external_attitude(const mavlink_message_t * const msg)

comms/mavlink/mavlink.h

@@ -112,6 +112,11 @@ class Mavlink : public CommLinkInterface
   bool initialized_ = false;
 
   ListenerInterface * listener_ = nullptr;
+
+  // Timing experiment
+  bool received_offboard_ctrl_ = false;
+  mavlink_message_t prev_offboard_msg_;
+  void send_previous_offboard_control() override;
 };
 
 } // namespace rosflight_firmware

include/comm_link.h

@@ -167,6 +167,9 @@ class CommLinkInterface
 
   // register listener
   virtual void set_listener(ListenerInterface * listener) = 0;
+
+  // Timing experiments
+  virtual void send_previous_offboard_control() = 0;
 };
 
 } // namespace rosflight_firmware

src/comm_manager.cpp

@@ -510,6 +510,7 @@ void CommManager::stream(got_flags got)
 
   send_buffered_log_messages();
 
+  comm_link_.send_previous_offboard_control();
 }
 
 void CommManager::send_next_param(void)