AP_DDS: Added individual motor control

ArduCopter/AP_ExternalControl_Copter.cpp

@@ -38,6 +38,19 @@ bool AP_ExternalControl_Copter::set_global_position(const Location& loc)
     return copter.set_target_location(loc);
 }
 
+/*
+  sets actuator output.
+*/
+bool AP_ExternalControl_Copter::set_actuator_output(float actuator[AP_MOTORS_MAX_NUM_MOTORS])
+{
+    if (!ready_for_external_control()) {
+        return false;
+    }
+
+    copter.mode_guided.set_actuator_mode(actuator);
+    return true;
+}
+
 bool AP_ExternalControl_Copter::ready_for_external_control()
 {
     return copter.flightmode->in_guided_mode() && copter.motors->armed();

ArduCopter/AP_ExternalControl_Copter.h

@@ -21,6 +21,11 @@ class AP_ExternalControl_Copter : public AP_ExternalControl
       Sets the target global position for a loiter point.
     */
     bool set_global_position(const Location& loc) override WARN_IF_UNUSED;
+
+    /*
+      Sets actuator output.
+    */
+    bool set_actuator_output(float actuator[]) override WARN_IF_UNUSED;
 private:
     /*
       Return true if Copter is ready to handle external control data.

ArduCopter/GCS_MAVLink_Copter.cpp

@@ -154,6 +154,7 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned()
 
     switch (guided_mode) {
     case ModeGuided::SubMode::Angle:
+    case ModeGuided::SubMode::Actuator:
         // we don't have a local target when in angle mode
         return;
     case ModeGuided::SubMode::TakeOff:

ArduCopter/mode.h

@@ -1067,6 +1067,7 @@ class ModeGuided : public Mode {
     bool is_autopilot() const override { return true; }
     bool has_user_takeoff(bool must_navigate) const override { return true; }
     bool in_guided_mode() const override { return true; }
+    void output_to_motors() override;
 
     bool requires_terrain_failsafe() const override { return true; }
 
@@ -1093,6 +1094,7 @@ class ModeGuided : public Mode {
     void set_accel(const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
     void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
     void set_velaccel(const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
+    void set_actuator_mode(float actuator[]);
     bool set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
     bool set_destination_posvelaccel(const Vector3f& destination, const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
 
@@ -1113,6 +1115,7 @@ class ModeGuided : public Mode {
     bool limit_check();
 
     bool is_taking_off() const override;
+    bool can_set_auto_arm(float actuator_values[]);
 
     bool set_speed_xy(float speed_xy_cms) override;
     bool set_speed_up(float speed_up_cms) override;
@@ -1130,6 +1133,7 @@ class ModeGuided : public Mode {
         VelAccel,
         Accel,
         Angle,
+        Actuator,
     };
 
     SubMode submode() const { return guided_mode; }
@@ -1185,11 +1189,13 @@ class ModeGuided : public Mode {
     void pos_control_start();
     void accel_control_start();
     void velaccel_control_start();
+    void motor_control_start();
     void posvelaccel_control_start();
     void takeoff_run();
     void pos_control_run();
     void accel_control_run();
     void velaccel_control_run();
+    void motor_control_run();
     void pause_control_run();
     void posvelaccel_control_run();
     void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);

ArduCopter/mode_guided.cpp

@@ -23,6 +23,11 @@ struct {
     bool use_thrust;
 } static guided_angle_state;
 
+struct {
+    uint32_t update_time_ms;
+    float actuator[AP_MOTORS_MAX_NUM_MOTORS];
+} static guided_motor_state;
+
 struct Guided_Limit {
     uint32_t timeout_ms;  // timeout (in seconds) from the time that guided is invoked
     float alt_min_cm;   // lower altitude limit in cm above home (0 = no limit)
@@ -102,6 +107,10 @@ void ModeGuided::run()
     case SubMode::Angle:
         angle_control_run();
         break;
+    case SubMode::Actuator:
+        motor_control_run();
+        break;
+
     }
  }
 
@@ -284,6 +293,11 @@ void ModeGuided::posvelaccel_control_start()
     pva_control_start();
 }
 
+void ModeGuided::motor_control_start()
+{
+    guided_mode = SubMode::Actuator;
+}
+
 bool ModeGuided::is_taking_off() const
 {
     return guided_mode == SubMode::TakeOff && !takeoff_complete;
@@ -430,6 +444,7 @@ bool ModeGuided::get_wp(Location& destination) const
     case SubMode::Accel:
     case SubMode::VelAccel:
     case SubMode::PosVelAccel:
+    case SubMode::Actuator:
         break;
     }
 
@@ -584,6 +599,41 @@ void ModeGuided::set_velaccel(const Vector3f& velocity, const Vector3f& accelera
 #endif
 }
 
+// set_actuator_mode - sets guided mode's individual motor mode
+void ModeGuided::set_actuator_mode(float actuator[AP_MOTORS_MAX_NUM_MOTORS]) {
+#if FRAME_CONFIG == MULTICOPTER_FRAME
+    if (guided_mode != SubMode::Actuator) {
+        motor_control_start();
+    }
+
+    guided_motor_state.update_time_ms = millis();
+    memcpy(guided_motor_state.actuator, actuator, AP_MOTORS_MAX_NUM_MOTORS * sizeof(float));
+#else
+    guided_motor_state.update_time_ms = millis();
+    return;
+#endif
+}
+
+// output_to_motor - send output to the motors
+void ModeGuided::output_to_motors()
+{
+#if FRAME_CONFIG == MULTICOPTER_FRAME
+    if (guided_mode == SubMode::Actuator){
+        for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
+            if (!motors->is_motor_enabled(i)) {
+                continue;
+            }
+            guided_motor_state.actuator[i] = constrain_float(guided_motor_state.actuator[i], 0, 1);
+            motors->rc_write(i, motors->output_to_pwm(guided_motor_state.actuator[i]));
+        }
+        return;
+    }
+#endif
+
+    motors->output();
+
+}
+
 // set_destination_posvel - set guided mode position and velocity target
 bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
 {
@@ -853,6 +903,28 @@ void ModeGuided::velaccel_control_run()
     attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
+// checks for incoming actuator commands
+void ModeGuided::motor_control_run()
+{
+#if FRAME_CONFIG == MULTICOPTER_FRAME
+    // set actuator values to zero if no updates received for 3 seconds
+    uint32_t tnow = millis();
+    if (tnow - guided_motor_state.update_time_ms > get_timeout_ms()) {
+        for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
+            if (!motors->is_motor_enabled(i)) {
+                continue;
+            }
+
+            guided_motor_state.actuator[i] = 0;
+        }
+        // TODO: Add fallback to POS HOLD or other control in case no updates received if the vehicle is flying
+    }
+#else
+    guided_motor_state.update_time_ms = millis();
+    return;
+#endif
+}
+
 // pause_control_run - runs the guided mode pause controller
 // called from guided_run
 void ModeGuided::pause_control_run()
@@ -1141,6 +1213,7 @@ int32_t ModeGuided::wp_bearing() const
     case SubMode::Accel:
     case SubMode::VelAccel:
     case SubMode::Angle:
+    case SubMode::Actuator:
         // these do not have bearings
         return 0;
     }
@@ -1160,6 +1233,7 @@ float ModeGuided::crosstrack_error() const
     case SubMode::PosVelAccel:
         return pos_control->crosstrack_error();
     case SubMode::Angle:
+    case SubMode::Actuator:
         // no track to have a crosstrack to
         return 0;
     }

Tools/ros2/ardupilot_msgs/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(rosidl_default_generators REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/GlobalPosition.msg"
   "msg/Status.msg"
+  "msg/MotorControl.msg"
   "srv/ArmMotors.srv"
   "srv/ModeSwitch.srv"
   "srv/Takeoff.srv"

Tools/ros2/ardupilot_msgs/msg/MotorControl.msg

@@ -0,0 +1,2 @@
+builtin_interfaces/Time timestamp # ms
+float32[12] actuator # [-1, 1]

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -97,6 +97,9 @@ geometry_msgs_msg_TwistStamped AP_DDS_Client::rx_velocity_control_topic {};
 #if AP_DDS_GLOBAL_POS_CTRL_ENABLED
 ardupilot_msgs_msg_GlobalPosition AP_DDS_Client::rx_global_position_control_topic {};
 #endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED
+#if AP_DDS_MOT_CTRL_ENABLED
+ardupilot_msgs_msg_MotorControl AP_DDS_Client::rx_motor_control_topic {};
+#endif // AP_DDS_MOT_CTRL_ENABLED
 
 // Define the parameter server data members, which are static class scope.
 // If these are created on the stack, then the AP_DDS_Client::on_request
@@ -837,6 +840,19 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
         break;
     }
 #endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED
+#if AP_DDS_MOT_CTRL_ENABLED
+    case topics[to_underlying(TopicIndex::MOTOR_CONTROL_SUB)].dr_id.id: {
+        const bool success = ardupilot_msgs_msg_MotorControl_deserialize_topic(ub, &rx_motor_control_topic);
+        if (success == false) {
+            break;
+        }
+#if AP_EXTERNAL_CONTROL_ENABLED
+        if (!AP_DDS_External_Control::handle_motor_control(rx_motor_control_topic)) {
+        }
+#endif // AP_EXTERNAL_CONTROL_ENABLED
+        break;
+    }
+#endif // AP_DDS_MOT_CTRL_ENABLED
     }
 
 }

libraries/AP_DDS/AP_DDS_Client.h

@@ -10,6 +10,9 @@
 #if AP_DDS_GLOBAL_POS_CTRL_ENABLED
 #include "ardupilot_msgs/msg/GlobalPosition.h"
 #endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED
+#if AP_DDS_MOT_CTRL_ENABLED
+#include "ardupilot_msgs/msg/MotorControl.h"
+#endif //AP_DDS_MOT_CTRL_ENABLED
 #if AP_DDS_TIME_PUB_ENABLED
 #include "builtin_interfaces/msg/Time.h"
 #endif // AP_DDS_TIME_PUB_ENABLED
@@ -227,6 +230,10 @@ class AP_DDS_Client
     // incoming REP147 goal interface global position
     static ardupilot_msgs_msg_GlobalPosition rx_global_position_control_topic;
 #endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED
+#if AP_DDS_MOT_CTRL_ENABLED
+    // incoming motor control
+    static ardupilot_msgs_msg_MotorControl rx_motor_control_topic;
+#endif // AP_DDS_MOT_CTRL_ENABLED
 #if AP_DDS_DYNAMIC_TF_SUB_ENABLED
     // incoming transforms
     static tf2_msgs_msg_TFMessage rx_dynamic_transforms_topic;

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

@@ -85,6 +85,16 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta
     return false;
 }
 
+bool AP_DDS_External_Control::handle_motor_control(ardupilot_msgs_msg_MotorControl& cmd_mot)
+{
+    auto *external_control = AP::externalcontrol();
+    if (external_control == nullptr) {
+        return false;
+    }
+
+    return external_control->set_actuator_output(cmd_mot.actuator);
+}
+
 bool AP_DDS_External_Control::arm(AP_Arming::Method method, bool do_arming_checks)
 {
     auto *external_control = AP::externalcontrol();

libraries/AP_DDS/AP_DDS_ExternalControl.h

@@ -2,6 +2,7 @@
 
 #if AP_DDS_ENABLED
 #include "ardupilot_msgs/msg/GlobalPosition.h"
+#include "ardupilot_msgs/msg/MotorControl.h"
 #include "geometry_msgs/msg/TwistStamped.h"
 #include <AP_Arming/AP_Arming.h>
 #include <AP_Common/Location.h>
@@ -13,6 +14,8 @@ class AP_DDS_External_Control
     // https://ros.org/reps/rep-0147.html#goal-interface
     static bool handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos);
     static bool handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel);
+    static bool handle_motor_control(ardupilot_msgs_msg_MotorControl& cmd_mot);
+
     static bool arm(AP_Arming::Method method, bool do_arming_checks);
     static bool disarm(AP_Arming::Method method, bool do_disarm_checks);
 

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -66,6 +66,9 @@ enum class TopicIndex: uint8_t {
 #if AP_DDS_GLOBAL_POS_CTRL_ENABLED
     GLOBAL_POSITION_SUB,
 #endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED
+#if AP_DDS_MOT_CTRL_ENABLED
+    MOTOR_CONTROL_SUB,
+#endif // AP_DDS_MOT_CTRL_ENABLED
 };
 
 static inline constexpr uint8_t to_underlying(const TopicIndex index)
@@ -382,4 +385,22 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         },
     },
 #endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED
+#if AP_DDS_MOT_CTRL_ENABLED
+    {
+        .topic_id = to_underlying(TopicIndex::MOTOR_CONTROL_SUB),
+        .pub_id = to_underlying(TopicIndex::MOTOR_CONTROL_SUB),
+        .sub_id = to_underlying(TopicIndex::MOTOR_CONTROL_SUB),
+        .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::MOTOR_CONTROL_SUB), .type=UXR_DATAWRITER_ID},
+        .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::MOTOR_CONTROL_SUB), .type=UXR_DATAREADER_ID},
+        .topic_rw = Topic_rw::DataReader,
+        .topic_name = "rt/ap/cmd_mot",
+        .type_name = "ardupilot_msgs::msg::dds_::MotorControl_",
+        .qos = {
+            .durability = UXR_DURABILITY_VOLATILE,
+            .reliability = UXR_RELIABILITY_BEST_EFFORT,
+            .history = UXR_HISTORY_KEEP_LAST,
+            .depth = 5,
+        },
+    },
+#endif // AP_DDS_MOT_CTRL_ENABLED
 };

libraries/AP_DDS/AP_DDS_config.h

@@ -129,6 +129,10 @@
 #define AP_DDS_GLOBAL_POS_CTRL_ENABLED 1
 #endif
 
+#ifndef AP_DDS_MOT_CTRL_ENABLED
+#define AP_DDS_MOT_CTRL_ENABLED 1
+#endif
+
 #ifndef AP_DDS_DYNAMIC_TF_SUB_ENABLED
 #define AP_DDS_DYNAMIC_TF_SUB_ENABLED 1
 #endif

libraries/AP_DDS/Idl/ardupilot_msgs/msg/MotorControl.idl

@@ -0,0 +1,20 @@
+// generated from rosidl_adapter/resource/msg.idl.em
+// with input from ardupilot_msgs/msg/MotorControl.msg
+// generated code does not contain a copyright notice
+
+#include "builtin_interfaces/msg/Time.idl"
+
+module ardupilot_msgs {
+  module msg {
+    typedef float float__12[12];
+    struct MotorControl {
+      @verbatim (language="comment", text=
+        "ms")
+      builtin_interfaces::msg::Time timestamp;
+
+      @verbatim (language="comment", text=
+        "[-1, 1]")
+      float__12 actuator;
+    };
+  };
+};

libraries/AP_ExternalControl/AP_ExternalControl.h

@@ -33,6 +33,13 @@ class AP_ExternalControl
         return false;
     }
 
+    /*
+      Sets actuator output.
+     */
+    virtual bool set_actuator_output(float actuator[]) WARN_IF_UNUSED {
+        return false;
+    }
+
     /*
         Arm the vehicle
     */