From 08a5c3ef7c74528150245481828d8fe66f305969 Mon Sep 17 00:00:00 2001 From: snktshrma Date: Wed, 29 Jan 2025 11:30:45 +0530 Subject: [PATCH] AP_DDS: Added individual motor control --- ArduCopter/AP_ExternalControl_Copter.cpp | 13 ++++ ArduCopter/AP_ExternalControl_Copter.h | 5 ++ ArduCopter/GCS_MAVLink_Copter.cpp | 1 + ArduCopter/mode.h | 6 ++ ArduCopter/mode_guided.cpp | 73 +++++++++++++++++++ Tools/ros2/ardupilot_msgs/CMakeLists.txt | 1 + .../ros2/ardupilot_msgs/msg/MotorControl.msg | 3 + libraries/AP_DDS/AP_DDS_Client.cpp | 16 ++++ libraries/AP_DDS/AP_DDS_Client.h | 7 ++ libraries/AP_DDS/AP_DDS_ExternalControl.cpp | 10 +++ libraries/AP_DDS/AP_DDS_ExternalControl.h | 3 + libraries/AP_DDS/AP_DDS_Topic_Table.h | 21 ++++++ libraries/AP_DDS/AP_DDS_config.h | 4 + .../Idl/ardupilot_msgs/msg/MotorControl.idl | 22 ++++++ libraries/AP_DDS/README.md | 10 +++ .../AP_ExternalControl/AP_ExternalControl.h | 7 ++ libraries/AP_Motors/AP_MotorsMulticopter.h | 6 +- 17 files changed, 205 insertions(+), 3 deletions(-) create mode 100644 Tools/ros2/ardupilot_msgs/msg/MotorControl.msg create mode 100644 libraries/AP_DDS/Idl/ardupilot_msgs/msg/MotorControl.idl diff --git a/ArduCopter/AP_ExternalControl_Copter.cpp b/ArduCopter/AP_ExternalControl_Copter.cpp index eb16147737fd66..678d9cb144c10d 100644 --- a/ArduCopter/AP_ExternalControl_Copter.cpp +++ b/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(); diff --git a/ArduCopter/AP_ExternalControl_Copter.h b/ArduCopter/AP_ExternalControl_Copter.h index 527af304f64a7f..1047dd650ad613 100644 --- a/ArduCopter/AP_ExternalControl_Copter.h +++ b/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. diff --git a/ArduCopter/GCS_MAVLink_Copter.cpp b/ArduCopter/GCS_MAVLink_Copter.cpp index 1c0f2115d838c4..8acdc9d5690e09 100644 --- a/ArduCopter/GCS_MAVLink_Copter.cpp +++ b/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: diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index bc38167cbee529..f1de27cf3ddf18 100644 --- a/ArduCopter/mode.h +++ b/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); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index cfc8e27eff7476..187f6592f2ea2a 100644 --- a/ArduCopter/mode_guided.cpp +++ b/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,27 @@ 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 + if (millis() - 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 +1212,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 +1232,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; } diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt index 767b89a89b7691..8db46738b15767 100644 --- a/Tools/ros2/ardupilot_msgs/CMakeLists.txt +++ b/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" diff --git a/Tools/ros2/ardupilot_msgs/msg/MotorControl.msg b/Tools/ros2/ardupilot_msgs/msg/MotorControl.msg new file mode 100644 index 00000000000000..3d529a86d838ba --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/msg/MotorControl.msg @@ -0,0 +1,3 @@ +# Control the direct motor outputs of the vehicle +builtin_interfaces/Time timestamp # ms +float32[32] actuator # [-1, 1] diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 87d54222e76ec4..987bda4fd55f97 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/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 } } diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 3a3678412ea0a7..77cbb3c6a997d8 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/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; diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp index 259dcb51601c9a..1a385ae18452b3 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp +++ b/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(); diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.h b/libraries/AP_DDS/AP_DDS_ExternalControl.h index eeb86e5e9c1325..d8c983501aeee2 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.h +++ b/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 #include @@ -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); diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index d471a254375868..20afe91d159812 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/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/experimental/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 }; diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index 3a68a349d5a3c8..6251b4ba5e9dfe 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -26,6 +26,10 @@ #define AP_DDS_IMU_PUB_ENABLED AP_DDS_EXPERIMENTAL_ENABLED #endif +#ifndef AP_DDS_MOT_CTRL_ENABLED +#define AP_DDS_MOT_CTRL_ENABLED AP_DDS_EXPERIMENTAL_ENABLED +#endif + #ifndef AP_DDS_DELAY_IMU_TOPIC_MS #define AP_DDS_DELAY_IMU_TOPIC_MS 5 #endif diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/msg/MotorControl.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/MotorControl.idl new file mode 100644 index 00000000000000..ab7e2a5b5f4bc0 --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/MotorControl.idl @@ -0,0 +1,22 @@ +// 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__32[32]; + @verbatim (language="comment", text= + "Control the direct motor outputs of the vehicle") + struct MotorControl { + @verbatim (language="comment", text= + "ms") + builtin_interfaces::msg::Time timestamp; + + @verbatim (language="comment", text= + "[-1, 1]") + float__32 actuator; + }; + }; +}; \ No newline at end of file diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 04055bdb11e09d..1674d159759db1 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -170,6 +170,7 @@ Published topics: Subscribed topics: * /ap/cmd_gps_pose [ardupilot_msgs/msg/GlobalPosition] 1 subscriber * /ap/cmd_vel [geometry_msgs/msg/TwistStamped] 1 subscriber + * /ap/experimental/cmd_mot [ardupilot_msgs/msg/MotorControl] 1 subscriber * /ap/joy [sensor_msgs/msg/Joy] 1 subscriber * /ap/tf [tf2_msgs/msg/TFMessage] 1 subscriber ``` @@ -291,6 +292,15 @@ ros2 topic pub /ap/cmd_gps_pose ardupilot_msgs/msg/GlobalPosition "{latitude: 34 publisher: beginning loop publishing #1: ardupilot_msgs.msg.GlobalPosition(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), coordinate_frame=0, type_mask=0, latitude=34.0, longitude=118.0, altitude=1000.0, velocity=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)), acceleration_or_force=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)), yaw=0.0) ``` +- `/ap/experimental/cmd_mot` (type `ardupilot_msgs/msg/MotorControl`): sends +direct actuator control output to vehicle. + +```bash +ros2 topic pub /ap/experimental/cmd_mot ardupilot_msgs/msg/MotorControl "{timestamp: {sec: 0, nanosec: 0}, actuator: [0.7, 0.7, 0.7, 0.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}" + +publisher: beginning loop +publishing #1: ardupilot_msgs.msg.MotorControl(timestamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), actuator=array([0.7, 0.7, 0.7, 0.7, 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. ], dtype=float32)) +``` ## Contributing to `AP_DDS` library diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.h b/libraries/AP_ExternalControl/AP_ExternalControl.h index 57670008f48968..afe0baf019b9bb 100644 --- a/libraries/AP_ExternalControl/AP_ExternalControl.h +++ b/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 */ diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 7864f9b57acbdd..3a8947b88b1640 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -92,6 +92,9 @@ class AP_MotorsMulticopter : public AP_Motors { // return thrust for motor motor_num, returns true if value is valid false otherwise bool get_thrust(uint8_t motor_num, float& thr_out) const override; + // convert actuator output (0~1) range to pwm range + int16_t output_to_pwm(float _actuator_output); + #if HAL_LOGGING_ENABLED // 10hz logging of voltage scaling and max trust void Log_Write() override; @@ -124,9 +127,6 @@ class AP_MotorsMulticopter : public AP_Motors { // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max virtual float get_current_limit_max_throttle(); - // convert actuator output (0~1) range to pwm range - int16_t output_to_pwm(float _actuator_output); - // adds slew rate limiting to actuator output if MOT_SLEW_TIME > 0 and not shutdown void set_actuator_with_slew(float& actuator_output, float input);