AP_ExternalControl_Copter: Refactor ready for control

* Refactor ready state to a single clearer function * It can be re-used for all controls API's Signed-off-by: Ryan Friedman <[email protected]>
tridge · Aug 10, 2023 · 7f56e7b · 7f56e7b
1 parent 911c236
commit 7f56e7b
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 1 deletion.
diff --git a/ArduCopter/AP_ExternalControl_Copter.cpp b/ArduCopter/AP_ExternalControl_Copter.cpp
@@ -14,7 +14,7 @@
 */
 bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads)
 {
-    if (!copter.flightmode->in_guided_mode() || !copter.motors->armed()) {
+    if (!ready_for_external_control()) {
         return false;
     }
     const float yaw_rate_cds = isnan(yaw_rate_rads)? 0: degrees(yaw_rate_rads)*100;
@@ -29,4 +29,9 @@ bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f
     return true;
 }
 
+bool AP_ExternalControl_Copter::ready_for_external_control()
+{
+    return copter.flightmode->in_guided_mode() && copter.motors->armed();
+}
+
 #endif // AP_EXTERNAL_CONTROL_ENABLED
diff --git a/ArduCopter/AP_ExternalControl_Copter.h b/ArduCopter/AP_ExternalControl_Copter.h
@@ -15,6 +15,12 @@ class AP_ExternalControl_Copter : public AP_ExternalControl {
       Yaw is in earth frame, NED [rad/s].
      */
     bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads) override;
+private:
+    /*
+      Return true if Copter is ready to handle external control data.
+      Currently checks mode and arm states.
+    */
+    bool ready_for_external_control();
 };
 
 #endif // AP_EXTERNAL_CONTROL_ENABLED