From e72467c00d7a8d0ded765a6e4c1417c29c4e280e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Aug 2023 10:28:00 +1000 Subject: [PATCH 1/7] AP_ExternalControl: external control library for MAVLink,lua and DDS Signed-off-by: Ryan Friedman Co-authored-by: Andrew Tridgell --- .../AP_ExternalControl/AP_ExternalControl.cpp | 24 +++++++++++ .../AP_ExternalControl/AP_ExternalControl.h | 43 +++++++++++++++++++ .../AP_ExternalControl_config.h | 10 +++++ 3 files changed, 77 insertions(+) create mode 100644 libraries/AP_ExternalControl/AP_ExternalControl.cpp create mode 100644 libraries/AP_ExternalControl/AP_ExternalControl.h create mode 100644 libraries/AP_ExternalControl/AP_ExternalControl_config.h diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.cpp b/libraries/AP_ExternalControl/AP_ExternalControl.cpp new file mode 100644 index 0000000000000..e0e92f0a29911 --- /dev/null +++ b/libraries/AP_ExternalControl/AP_ExternalControl.cpp @@ -0,0 +1,24 @@ +#include "AP_ExternalControl.h" + +#if AP_EXTERNAL_CONTROL_ENABLED + +// singleton instance +AP_ExternalControl *AP_ExternalControl::singleton; + +AP_ExternalControl::AP_ExternalControl() +{ + singleton = this; +} + + +namespace AP +{ + +AP_ExternalControl *externalcontrol() +{ + return AP_ExternalControl::get_singleton(); +} + +}; + +#endif // AP_EXTERNAL_CONTROL_ENABLED diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.h b/libraries/AP_ExternalControl/AP_ExternalControl.h new file mode 100644 index 0000000000000..7c3d7b7e35728 --- /dev/null +++ b/libraries/AP_ExternalControl/AP_ExternalControl.h @@ -0,0 +1,43 @@ +/* + external control library for MAVLink, DDS and scripting + */ + +#pragma once + +#include "AP_ExternalControl_config.h" + +#if AP_EXTERNAL_CONTROL_ENABLED + +#include + +class AP_ExternalControl +{ +public: + + AP_ExternalControl(); + /* + 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]. + */ + virtual bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads) + { + return false; + } + + static AP_ExternalControl *get_singleton(void) + { + return singleton; + } + +private: + static AP_ExternalControl *singleton; +}; + + +namespace AP +{ +AP_ExternalControl *externalcontrol(); +}; + +#endif // AP_EXTERNAL_CONTROL_ENABLED diff --git a/libraries/AP_ExternalControl/AP_ExternalControl_config.h b/libraries/AP_ExternalControl/AP_ExternalControl_config.h new file mode 100644 index 0000000000000..1192bac86717c --- /dev/null +++ b/libraries/AP_ExternalControl/AP_ExternalControl_config.h @@ -0,0 +1,10 @@ +#pragma once + +#include +#include + +#ifndef AP_EXTERNAL_CONTROL_ENABLED +#define AP_EXTERNAL_CONTROL_ENABLED 1 +#endif + + From d409f5d17251235176046e880d2342621835509d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Aug 2023 10:28:58 +1000 Subject: [PATCH 2/7] Copter: added AP_ExternalControl support Signed-off-by: Ryan Friedman Co-authored-by: Andrew Tridgell --- ArduCopter/AP_ExternalControl_Copter.cpp | 37 ++++++++++++++++++++++++ ArduCopter/AP_ExternalControl_Copter.h | 26 +++++++++++++++++ ArduCopter/Copter.h | 14 +++++++++ ArduCopter/mode.h | 5 ++++ 4 files changed, 82 insertions(+) create mode 100644 ArduCopter/AP_ExternalControl_Copter.cpp create mode 100644 ArduCopter/AP_ExternalControl_Copter.h diff --git a/ArduCopter/AP_ExternalControl_Copter.cpp b/ArduCopter/AP_ExternalControl_Copter.cpp new file mode 100644 index 0000000000000..e5f1f49ec7421 --- /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 0000000000000..e9a879106ebef --- /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 3afe7f0f4b26e..bb3942a8f4822 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 @@ class Copter : public AP_Vehicle { 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 @@ class Copter : public AP_Vehicle { 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 b4e99d60f4f8b..28c78411797a8 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 @@ class ModeFlowHold : public Mode { 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; } From 386b0578488b5aab60c10181fd3e30423e5dcc01 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Aug 2023 10:41:19 +1000 Subject: [PATCH 3/7] waf: added AP_ExternalControl to set of libraries --- Tools/ardupilotwaf/ardupilotwaf.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 58ab8739f25a6..75c9975b826bd 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -115,6 +115,7 @@ 'AP_AIS', 'AP_OpenDroneID', 'AP_CheckFirmware', + 'AP_ExternalControl', ] def get_legacy_defines(sketch_name, bld): From 58b3319c60835a729d7c5a380be459539b996bd3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Aug 2023 10:41:46 +1000 Subject: [PATCH 4/7] AP_DDS: use AP_ExternalControl for velocity control Signed-off-by: Ryan Friedman Co-authored-by: Andrew Tridgell --- libraries/AP_DDS/AP_DDS_Client.cpp | 16 +++++++---- libraries/AP_DDS/AP_DDS_ExternalControl.cpp | 32 +++++++++++++++++++++ libraries/AP_DDS/AP_DDS_ExternalControl.h | 11 +++++++ libraries/AP_DDS/AP_DDS_Frames.h | 7 +++++ 4 files changed, 60 insertions(+), 6 deletions(-) create mode 100644 libraries/AP_DDS/AP_DDS_ExternalControl.cpp create mode 100644 libraries/AP_DDS/AP_DDS_ExternalControl.h create mode 100644 libraries/AP_DDS/AP_DDS_Frames.h diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 5c2933f2d5446..0d7b26d7b4ae1 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -10,6 +10,11 @@ #include #include #include +#include +#if AP_EXTERNAL_CONTROL_ENABLED +#include "AP_DDS_ExternalControl.h" +#endif +#include "AP_DDS_Frames.h" #include "AP_DDS_Client.h" #include "AP_DDS_Topic_Table.h" @@ -24,10 +29,6 @@ static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33; static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = 33; static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33; static constexpr uint16_t DELAY_CLOCK_TOPIC_MS = 10; -static char WGS_84_FRAME_ID[] = "WGS-84"; - -// https://www.ros.org/reps/rep-0105.html#base-link -static char BASE_LINK_FRAME_ID[] = "base_link"; // Define the subscriber data members, which are static class scope. // If these are created on the stack in the subscriber, @@ -475,8 +476,11 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin } subscribe_sample_count++; - GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received geometry_msgs/TwistStamped"); - // TODO implement the velocity control to AP +#if AP_EXTERNAL_CONTROL_ENABLED + if (!AP_DDS_External_Control::handle_velocity_control(rx_velocity_control_topic)) { + // TODO #23430 handle velocity control failure through rosout, throttled. + } +#endif // AP_EXTERNAL_CONTROL_ENABLED break; } } diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp new file mode 100644 index 0000000000000..8d551cac1375c --- /dev/null +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp @@ -0,0 +1,32 @@ +#if AP_DDS_ENABLED + +#include "AP_DDS_ExternalControl.h" +#include "AP_DDS_Frames.h" + +#include + +bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel) +{ + auto *external_control = AP::externalcontrol(); + if (external_control == nullptr) { + return false; + } + if (strcmp(cmd_vel.header.frame_id, MAP_FRAME) != 0) { + // Although REP-147 says cmd_vel should be in body frame, all the AP math is done in earth frame. + // This is because accounting for the gravity vector. + // Although the ROS 2 interface could support body-frame velocity control in the future, + // it is currently not supported. + return false; + } + + // Convert commands from ENU to NED frame + Vector3f linear_velocity { + float(cmd_vel.twist.linear.y), + float(cmd_vel.twist.linear.x), + float(-cmd_vel.twist.linear.z) }; + const float yaw_rate = -cmd_vel.twist.angular.z; + return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); +} + + +#endif // AP_DDS_ENABLED \ No newline at end of file diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.h b/libraries/AP_DDS/AP_DDS_ExternalControl.h new file mode 100644 index 0000000000000..dbffafdd81ad1 --- /dev/null +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.h @@ -0,0 +1,11 @@ +#pragma once + +#if AP_DDS_ENABLED +#include "geometry_msgs/msg/TwistStamped.h" + +class AP_DDS_External_Control +{ +public: + static bool handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel); +}; +#endif // AP_DDS_ENABLED diff --git a/libraries/AP_DDS/AP_DDS_Frames.h b/libraries/AP_DDS/AP_DDS_Frames.h new file mode 100644 index 0000000000000..0bdaf354ec116 --- /dev/null +++ b/libraries/AP_DDS/AP_DDS_Frames.h @@ -0,0 +1,7 @@ +#pragma once + +static constexpr char WGS_84_FRAME_ID[] = "WGS-84"; +// https://www.ros.org/reps/rep-0105.html#base-link +static constexpr char BASE_LINK_FRAME_ID[] = "base_link"; +// https://www.ros.org/reps/rep-0105.html#map +static constexpr char MAP_FRAME[] = "map"; From 5f46526f26506ef474c3e6472fd4098fe4a0e640 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Aug 2023 10:45:57 +1000 Subject: [PATCH 5/7] Plane: dummy AP_ExternalControl implementation --- ArduPlane/Plane.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 84c18d207ca55..5369536a43446 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -83,6 +83,10 @@ #include #include // Landing Gear library #include +#include +#if AP_EXTERNAL_CONTROL_ENABLED +#include +#endif #include "GCS_Mavlink.h" #include "GCS_Plane.h" @@ -770,6 +774,11 @@ class Plane : public AP_Vehicle { AP_Param param_loader {var_info}; + // dummy implementation of external control +#if AP_EXTERNAL_CONTROL_ENABLED + AP_ExternalControl external_control; +#endif + static const AP_Scheduler::Task scheduler_tasks[]; static const AP_Param::Info var_info[]; From 31a92c64d6a5f2fda06343d2d5ef4a2fac359ba2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Aug 2023 10:47:59 +1000 Subject: [PATCH 6/7] Rover: dummy implementation of AP_ExternalControl --- Rover/Rover.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/Rover/Rover.h b/Rover/Rover.h index d0a6f15ad35e7..fd22073c5bbbd 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -44,6 +44,10 @@ #include #include #include +#include +#if AP_EXTERNAL_CONTROL_ENABLED +#include +#endif // Configuration #include "defines.h" @@ -143,6 +147,11 @@ class Rover : public AP_Vehicle { // Arming/Disarming management class AP_Arming_Rover arming; + // dummy external control implementation +#if AP_EXTERNAL_CONTROL_ENABLED + AP_ExternalControl external_control; +#endif + #if AP_OPTICALFLOW_ENABLED AP_OpticalFlow optflow; #endif From b9afe37a00c221ee0f875cc97b99df136024c261 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Thu, 17 Aug 2023 16:01:51 -0600 Subject: [PATCH 7/7] Tools: Enforce astyle on AP_ExternalControl Signed-off-by: Ryan Friedman --- Tools/scripts/run_astyle.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/scripts/run_astyle.py b/Tools/scripts/run_astyle.py index ecf78a19e1ca4..83d784258f6d9 100755 --- a/Tools/scripts/run_astyle.py +++ b/Tools/scripts/run_astyle.py @@ -21,6 +21,7 @@ def __init__(self): self.retcode = 0 self.directories_to_check = [ 'libraries/AP_DDS', + 'libraries/AP_ExternalControl' ] self.files_to_check = []