diff --git a/ArduCopter/AP_ExternalControl_Copter.cpp b/ArduCopter/AP_ExternalControl_Copter.cpp new file mode 100644 index 0000000000..e5f1f49ec7 --- /dev/null +++ b/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 diff --git a/ArduCopter/AP_ExternalControl_Copter.h b/ArduCopter/AP_ExternalControl_Copter.h new file mode 100644 index 0000000000..e9a879106e --- /dev/null +++ b/ArduCopter/AP_ExternalControl_Copter.h @@ -0,0 +1,26 @@ +/* + external control library for copter + */ +#pragma once + +#include + +#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 diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 3afe7f0f4b..bb3942a8f4 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -100,6 +100,11 @@ #include "AP_Rally.h" // Rally point library #include "AP_Arming.h" +#include +#if AP_EXTERNAL_CONTROL_ENABLED +#include "AP_ExternalControl_Copter.h" +#endif + #include #if AP_BEACON_ENABLED #include @@ -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; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index b4e99d60f4..28c7841179 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -2,6 +2,7 @@ #include "Copter.h" #include +#include // 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; }