fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 04:30:36 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+39
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.