Copter: added AP_ExternalControl support

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
Co-authored-by: Andrew Tridgell <tridge60@gmail.com>

ArduCopter/AP_ExternalControl_Copter.cpp

@@ -0,0 +1,37 @@
+/*
+  external control library for copter
+ */
+
+
+#include "AP_ExternalControl_Copter.h"
+#if AP_EXTERNAL_CONTROL_ENABLED
+
+#include "Copter.h"
+
+/*
+  set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw
+  velocity is in earth frame, NED, m/s
+*/
+bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads)
+{
+    if (!ready_for_external_control()) {
+        return false;
+    }
+    const float yaw_rate_cds = isnan(yaw_rate_rads)? 0: degrees(yaw_rate_rads)*100;
+
+    // Copter velocity is positive if aicraft is moving up which is opposite the incoming NED frame.
+    Vector3f velocity_NEU_ms {
+        linear_velocity.x,
+        linear_velocity.y,
+        -linear_velocity.z };
+    Vector3f velocity_up_cms = velocity_NEU_ms * 100;
+    copter.mode_guided.set_velocity(velocity_up_cms, false, 0, !isnan(yaw_rate_rads), yaw_rate_cds);
+    return true;
+}
+
+bool AP_ExternalControl_Copter::ready_for_external_control()
+{
+    return copter.flightmode->in_guided_mode() && copter.motors->armed();
+}
+
+#endif // AP_EXTERNAL_CONTROL_ENABLED

ArduCopter/AP_ExternalControl_Copter.h

@@ -0,0 +1,26 @@
+/*
+  external control library for copter
+ */
+#pragma once
+
+#include <AP_ExternalControl/AP_ExternalControl.h>
+
+#if AP_EXTERNAL_CONTROL_ENABLED
+
+class AP_ExternalControl_Copter : public AP_ExternalControl {
+public:
+    /*
+      Set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw.
+      Velocity is in earth frame, NED [m/s].
+      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

ArduCopter/Copter.h

@@ -100,6 +100,11 @@
 #include "AP_Rally.h"           // Rally point library
 #include "AP_Arming.h"
 
+#include <AP_ExternalControl/AP_ExternalControl_config.h>
+#if AP_EXTERNAL_CONTROL_ENABLED
+#include "AP_ExternalControl_Copter.h"
+#endif
+
 #include <AP_Beacon/AP_Beacon_config.h>
 #if AP_BEACON_ENABLED
  #include <AP_Beacon/AP_Beacon.h>
@@ -193,6 +198,9 @@ public:
     friend class AP_AdvancedFailsafe_Copter;
 #endif
     friend class AP_Arming_Copter;
+#if AP_EXTERNAL_CONTROL_ENABLED
+    friend class AP_ExternalControl_Copter;
+#endif
     friend class ToyMode;
     friend class RC_Channel_Copter;
     friend class RC_Channels_Copter;
@@ -319,6 +327,12 @@ private:
     AP_OpticalFlow optflow;
 #endif
 
+    // external control library
+#if AP_EXTERNAL_CONTROL_ENABLED
+    AP_ExternalControl_Copter external_control;
+#endif
+
+
     // system time in milliseconds of last recorded yaw reset from ekf
     uint32_t ekfYawReset_ms;
     int8_t ekf_primary_core;

ArduCopter/mode.h

@@ -2,6 +2,7 @@
 
 #include "Copter.h"
 #include <AP_Math/chirp.h>
+#include <AP_ExternalControl/AP_ExternalControl_config.h> // TODO why is this needed if Copter.h includes this
 class Parameters;
 class ParametersG2;
 
@@ -957,6 +958,10 @@ private:
 class ModeGuided : public Mode {
 
 public:
+#if AP_EXTERNAL_CONTROL_ENABLED
+    friend class AP_ExternalControl_Copter;
+#endif
+
     // inherit constructor
     using Mode::Mode;
     Number mode_number() const override { return Number::GUIDED; }