fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 20:09:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: add support for DO_MOTOR_TEST

Browse Source 
- 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
This commit is contained in:
Beat Küng 2019-10-21 12:12:07 +02:00
parent eb6a9bd488
commit 285ae608a5
7 changed files with 91 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
msg/test_motor.msg
View File

@ -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)

1
msg/vehicle_command.msg
View File

@ -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|

30
src/lib/mixer_module/mixer_module.cpp
View File

@ -253,9 +253,21 @@ unsigned MixingOutput::motorTest()
int idx = test_motor.motor_number;
if (idx < MAX_ACTUATORS) {
_current_output_value[reorderedMotorIndex(idx)] =
math::constrain<uint16_t>(_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<uint16_t>(_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;
}

1
src/lib/mixer_module/mixer_module.hpp
View File

@ -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;

39
src/modules/commander/Commander.cpp
View File

@ -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.

4
src/modules/commander/Commander.hpp
View File

@ -52,6 +52,7 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/test_motor.h>
// subscriptions
#include <uORB/Subscription.hpp>
@ -205,6 +206,8 @@ private:
bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
uORB::PublicationQueued<vehicle_command_ack_s> &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<actuator_armed_s> _armed_pub{ORB_ID(actuator_armed)};
uORB::Publication<commander_state_s> _commander_state_pub{ORB_ID(commander_state)};
uORB::Publication<vehicle_status_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
uORB::Publication<test_motor_s> _test_motor_pub{ORB_ID(test_motor)};
uORB::PublicationData<home_position_s> _home_pub{ORB_ID(home_position)};

21
src/systemcmds/motor_test/motor_test.cpp
View File

@ -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_s> 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);
}
}