From 21699935e803ba84eee65628e36a10f17ed76b01 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 5 Nov 2021 11:46:21 +0100 Subject: [PATCH] vehicle_command: add VEHICLE_CMD_ACTUATOR_TEST and VEHICLE_CMD_CONFIGURE_ACTUATOR --- msg/vehicle_command.msg | 2 + src/modules/commander/Commander.cpp | 57 +++++++++++++++++++++++++++++ src/modules/commander/Commander.hpp | 3 ++ 3 files changed, 62 insertions(+) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index e32e00922e..02e7c3cc8c 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -69,6 +69,8 @@ uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto M uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| +uint16 VEHICLE_CMD_ACTUATOR_TEST = 310 # Actuator testing command|value [-1,1]|timeout [s]|Empty|Empty|output function| +uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311 # Actuator configuration command|configuration|Empty|Empty|Empty|output function| uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 89f73a0db4..8e82458315 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1163,6 +1163,10 @@ Commander::handle_command(const vehicle_command_s &cmd) cmd_result = handle_command_motor_test(cmd); break; + case vehicle_command_s::VEHICLE_CMD_ACTUATOR_TEST: + cmd_result = handle_command_actuator_test(cmd); + break; + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { const int param1 = cmd.param1; @@ -1429,6 +1433,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN: case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW: case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE: + case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR: /* ignore commands that are handled by other parts of the system */ break; @@ -1495,6 +1500,58 @@ Commander::handle_command_motor_test(const vehicle_command_s &cmd) return vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } +unsigned +Commander::handle_command_actuator_test(const vehicle_command_s &cmd) +{ + if (_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) { + return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; + } + + actuator_test_s actuator_test{}; + actuator_test.timestamp = hrt_absolute_time(); + actuator_test.function = (int)(cmd.param5 + 0.5); + + if (actuator_test.function < 1000) { + const int first_motor_function = 1; // from MAVLink ACTUATOR_OUTPUT_FUNCTION + const int first_servo_function = 33; + + if (actuator_test.function >= first_motor_function + && actuator_test.function < first_motor_function + actuator_test_s::MAX_NUM_MOTORS) { + actuator_test.function = actuator_test.function - first_motor_function + actuator_test_s::FUNCTION_MOTOR1; + + } else if (actuator_test.function >= first_servo_function + && actuator_test.function < first_servo_function + actuator_test_s::MAX_NUM_SERVOS) { + actuator_test.function = actuator_test.function - first_servo_function + actuator_test_s::FUNCTION_SERVO1; + + } else { + return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + } + + } else { + actuator_test.function -= 1000; + } + + actuator_test.value = cmd.param1; + + actuator_test.action = actuator_test_s::ACTION_DO_CONTROL; + int timeout_ms = (int)(cmd.param2 * 1000.f + 0.5f); + + if (timeout_ms <= 0) { + actuator_test.action = actuator_test_s::ACTION_RELEASE_CONTROL; + + } else { + actuator_test.timeout_ms = timeout_ms; + } + + // enforce a timeout and a maximum limit + if (actuator_test.timeout_ms == 0 || actuator_test.timeout_ms > 3000) { + actuator_test.timeout_ms = 3000; + } + + _actuator_test_pub.publish(actuator_test); + return vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; +} + void Commander::executeActionRequest(const action_request_s &action_request) { arm_disarm_reason_t arm_disarm_reason{}; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 3b8733d17f..5725b5e6a0 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -48,6 +48,7 @@ // publications #include #include +#include #include #include #include @@ -149,6 +150,7 @@ private: bool handle_command(const vehicle_command_s &cmd); unsigned handle_command_motor_test(const vehicle_command_s &cmd); + unsigned handle_command_actuator_test(const vehicle_command_s &cmd); void executeActionRequest(const action_request_s &action_request); @@ -436,6 +438,7 @@ private: uORB::Publication _commander_state_pub{ORB_ID(commander_state)}; uORB::Publication _failure_detector_status_pub{ORB_ID(failure_detector_status)}; uORB::Publication _test_motor_pub{ORB_ID(test_motor)}; + uORB::Publication _actuator_test_pub{ORB_ID(actuator_test)}; uORB::Publication _control_mode_pub{ORB_ID(vehicle_control_mode)}; uORB::Publication _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)}; uORB::Publication _status_pub{ORB_ID(vehicle_status)};