fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 08:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

vehicle_command: add VEHICLE_CMD_ACTUATOR_TEST and VEHICLE_CMD_CONFIGURE_ACTUATOR

Browse Source
This commit is contained in:
Beat Küng
2021-11-05 11:46:21 +01:00
committed by Daniel Agar
parent 6fdcc43ea8
commit 21699935e8
3 changed files with 62 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+57
View File
@@ -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{};