From 285ae608a569bb22ea16c55a7447f88fdd403b57 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 21 Oct 2019 12:12:07 +0200 Subject: [PATCH] commander: add support for DO_MOTOR_TEST - add an optional timeout to test_motor - enforce a timeout when receiving DO_MOTOR_TEST - limitation: DO_MOTOR_TEST can only control the MAIN outputs --- msg/test_motor.msg | 9 +++--- msg/vehicle_command.msg | 1 + src/lib/mixer_module/mixer_module.cpp | 30 ++++++++++++++++-- src/lib/mixer_module/mixer_module.hpp | 1 + src/modules/commander/Commander.cpp | 39 ++++++++++++++++++++++++ src/modules/commander/Commander.hpp | 4 +++ src/systemcmds/motor_test/motor_test.cpp | 21 ++++++++----- 7 files changed, 91 insertions(+), 14 deletions(-) diff --git a/msg/test_motor.msg b/msg/test_motor.msg index 133d97a901..2dc6174996 100644 --- a/msg/test_motor.msg +++ b/msg/test_motor.msg @@ -1,12 +1,13 @@ uint64 timestamp # time since system start (microseconds) uint8 NUM_MOTOR_OUTPUTS = 8 -uint8 ACTION_STOP = 0 # stop motors (disable motor test mode) -uint8 ACTION_RUN = 1 # run motors (enable motor test mode) +uint8 ACTION_STOP = 0 # stop all motors (disable motor test mode) +uint8 ACTION_RUN = 1 # run motor(s) (enable motor test mode) uint8 action # one of ACTION_* (applies to all motors) -uint32 motor_number # number of motor to spin -float32 value # output power, range [0..1] +uint32 motor_number # number of motor to spin [0..N-1] +float32 value # output power, range [0..1], -1 to stop individual motor +uint32 timeout_ms # timeout in ms after which to exit test mode (if 0, do not time out) uint8 driver_instance # select output driver (for boards with multiple outputs, like IO+FMU) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 52033bbe5e..8765c8202e 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -51,6 +51,7 @@ uint16 VEHICLE_CMD_DO_MOUNT_CONTROL=205 # Mission command to control a camera uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206 # Mission command to set TRIG_DIST for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_FENCE_ENABLE=207 # Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_PARACHUTE=208 # Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_MOTOR_TEST=209 # motor test command |Instance (1, ...)| throttle type| throttle| timeout [s]| Motor count | Test order| Empty| uint16 VEHICLE_CMD_DO_INVERTED_FLIGHT=210 # Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL=214 # Mission command to set TRIG_INTERVAL for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220 # Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 35ac2d8b4f..2489273481 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -253,9 +253,21 @@ unsigned MixingOutput::motorTest() int idx = test_motor.motor_number; if (idx < MAX_ACTUATORS) { - _current_output_value[reorderedMotorIndex(idx)] = - math::constrain(_min_value[idx] + (uint16_t)((_max_value[idx] - _min_value[idx]) * test_motor.value), - _min_value[idx], _max_value[idx]); + if (test_motor.value < 0.f) { + _current_output_value[reorderedMotorIndex(idx)] = _disarmed_value[idx]; + + } else { + _current_output_value[reorderedMotorIndex(idx)] = + math::constrain(_min_value[idx] + (uint16_t)((_max_value[idx] - _min_value[idx]) * test_motor.value), + _min_value[idx], _max_value[idx]); + } + } + + if (test_motor.timeout_ms > 0) { + _motor_test.timeout = test_motor.timestamp + test_motor.timeout_ms * 1000; + + } else { + _motor_test.timeout = 0; } } @@ -263,6 +275,18 @@ unsigned MixingOutput::motorTest() had_update = true; } + // check for timeouts + if (_motor_test.timeout != 0 && hrt_absolute_time() > _motor_test.timeout) { + _motor_test.in_test_mode = false; + _motor_test.timeout = 0; + + for (int i = 0; i < MAX_ACTUATORS; ++i) { + _current_output_value[i] = _disarmed_value[i]; + } + + had_update = true; + } + return (_motor_test.in_test_mode || had_update) ? _max_num_outputs : 0; } diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 387cf36d5b..33a0f88b7e 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -264,6 +264,7 @@ private: struct MotorTest { uORB::Subscription test_motor_sub{ORB_ID(test_motor)}; bool in_test_mode{false}; + hrt_abstime timeout{0}; }; MotorTest _motor_test; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 17bd662507..0f9cfbc789 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1080,6 +1080,10 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ main_state_transition(*status_local, commander_state_s::MAIN_STATE_ORBIT, status_flags, &internal_state); break; + case vehicle_command_s::VEHICLE_CMD_DO_MOTOR_TEST: + cmd_result = handle_command_motor_test(cmd); + break; + case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: @@ -1129,6 +1133,41 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ return true; } +unsigned +Commander::handle_command_motor_test(const vehicle_command_s &cmd) +{ + if (armed.armed || (safety.safety_switch_available && !safety.safety_off)) { + return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; + } + + test_motor_s test_motor{}; + test_motor.timestamp = hrt_absolute_time(); + test_motor.motor_number = (int)(cmd.param1 + 0.5f) - 1; + int throttle_type = (int)(cmd.param2 + 0.5f); + if (throttle_type != 0) { // 0: MOTOR_TEST_THROTTLE_PERCENT + return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + } + int motor_count = (int) (cmd.param5 + 0.5); + if (motor_count > 1) { + return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + } + test_motor.action = test_motor_s::ACTION_RUN; + test_motor.value = math::constrain(cmd.param3 / 100.f, 0.f, 1.f); + if (test_motor.value < FLT_EPSILON) { + // the message spec is not clear on whether 0 means stop, but it should be closer to what a user expects + test_motor.value = -1.f; + } + test_motor.timeout_ms = (int)(cmd.param4 * 1000.f + 0.5f); + // enforce a timeout and a maximum limit + if (test_motor.timeout_ms == 0 || test_motor.timeout_ms > 3000) { + test_motor.timeout_ms = 3000; + } + test_motor.driver_instance = 0; // the mavlink command does not allow to specify the instance, so set to 0 for now + _test_motor_pub.publish(test_motor); + + return vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; +} + /** * @brief This function initializes the home position an altitude of the vehicle. This happens first time we get a good GPS fix and each * time the vehicle is armed with a good GPS fix. diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index a5fd4dcfd4..8d780c19fb 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -52,6 +52,7 @@ #include #include #include +#include // subscriptions #include @@ -205,6 +206,8 @@ private: bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed, uORB::PublicationQueued &command_ack_pub, bool *changed); + unsigned handle_command_motor_test(const vehicle_command_s &cmd); + bool set_home_position(); bool set_home_position_alt_only(); @@ -291,6 +294,7 @@ private: uORB::Publication _armed_pub{ORB_ID(actuator_armed)}; uORB::Publication _commander_state_pub{ORB_ID(commander_state)}; uORB::Publication _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)}; + uORB::Publication _test_motor_pub{ORB_ID(test_motor)}; uORB::PublicationData _home_pub{ORB_ID(home_position)}; diff --git a/src/systemcmds/motor_test/motor_test.cpp b/src/systemcmds/motor_test/motor_test.cpp index 8060b33a5e..dfbd525d7c 100644 --- a/src/systemcmds/motor_test/motor_test.cpp +++ b/src/systemcmds/motor_test/motor_test.cpp @@ -48,10 +48,10 @@ extern "C" __EXPORT int motor_test_main(int argc, char *argv[]); -static void motor_test(unsigned channel, float value, uint8_t driver_instance); +static void motor_test(unsigned channel, float value, uint8_t driver_instance, int timeout_ms); static void usage(const char *reason); -void motor_test(unsigned channel, float value, uint8_t driver_instance) +void motor_test(unsigned channel, float value, uint8_t driver_instance, int timeout_ms) { test_motor_s test_motor{}; test_motor.timestamp = hrt_absolute_time(); @@ -59,6 +59,7 @@ void motor_test(unsigned channel, float value, uint8_t driver_instance) test_motor.value = value; test_motor.action = value >= 0.f ? test_motor_s::ACTION_RUN : test_motor_s::ACTION_STOP; test_motor.driver_instance = driver_instance; + test_motor.timeout_ms = timeout_ms; uORB::PublicationQueued test_motor_pub{ORB_ID(test_motor)}; test_motor_pub.publish(test_motor); @@ -90,6 +91,7 @@ Note: this can only be used for drivers which support the motor_test uorb topic PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Set motor(s) to a specific output value"); PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 7, "Motor to test (0...7, all if not specified)", true); PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 100, "Power (0...100)", true); + PRINT_MODULE_USAGE_PARAM_INT('t', 0, 0, 100, "Timeout in seconds (default=no timeout)", true); PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 4, "driver instance", true); PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop all motors"); PRINT_MODULE_USAGE_COMMAND_DESCR("iterate", "Iterate all motors starting and stopping one after the other"); @@ -103,11 +105,12 @@ int motor_test_main(int argc, char *argv[]) float value = 0.0f; uint8_t driver_instance = 0; int ch; + int timeout_ms = 0; int myoptind = 1; const char *myoptarg = NULL; - while ((ch = px4_getopt(argc, argv, "i:m:p:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "i:m:p:t:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'i': @@ -131,6 +134,10 @@ int motor_test_main(int argc, char *argv[]) value = ((float)lval) / 100.f; break; + case 't': + timeout_ms = strtol(myoptarg, NULL, 0) * 1000; + break; + default: usage(NULL); return 1; @@ -148,9 +155,9 @@ int motor_test_main(int argc, char *argv[]) value = 0.15f; for (int i = 0; i < 8; ++i) { - motor_test(i, value, driver_instance); + motor_test(i, value, driver_instance, 0); px4_usleep(500000); - motor_test(i, -1.f, driver_instance); + motor_test(i, -1.f, driver_instance, 0); px4_usleep(10000); } @@ -171,12 +178,12 @@ int motor_test_main(int argc, char *argv[]) if (run_test) { if (channel < 0) { for (int i = 0; i < 8; ++i) { - motor_test(i, value, driver_instance); + motor_test(i, value, driver_instance, timeout_ms); px4_usleep(10000); } } else { - motor_test(channel, value, driver_instance); + motor_test(channel, value, driver_instance, timeout_ms); } }