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

commander: move answer_command() to class

Browse Source
This commit is contained in:
Daniel Agar
2020-12-02 10:20:50 +01:00
committed by Beat Küng
parent d5dc7e2873
commit a03b91c01e
2 changed files with 37 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+35 -43
View File
@@ -96,9 +96,6 @@ static struct vehicle_status_s status = {};
static struct vehicle_status_flags_s status_flags = {}; static struct vehicle_status_flags_s status_flags = {};
static void answer_command(const vehicle_command_s &cmd, unsigned result,
uORB::Publication<vehicle_command_ack_s> &command_ack_pub);
#if defined(BOARD_HAS_POWER_CONTROL) #if defined(BOARD_HAS_POWER_CONTROL)
static orb_advert_t power_button_state_pub = nullptr; static orb_advert_t power_button_state_pub = nullptr;
static int power_button_state_notification_cb(board_power_button_state_notification_e request) static int power_button_state_notification_cb(board_power_button_state_notification_e request)
@@ -474,8 +471,7 @@ Commander::Commander() :
} }
bool bool
Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local, Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local)
uORB::Publication<vehicle_command_ack_s> &command_ack_pub)
{ {
/* only handle commands that are meant to be handled by this system and component */ /* only handle commands that are meant to be handled by this system and component */
if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id) if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id)
@@ -988,13 +984,13 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
if (param1 == 0) { if (param1 == 0) {
// 0: Do nothing for autopilot // 0: Do nothing for autopilot
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
#if defined(CONFIG_BOARDCTL_RESET) #if defined(CONFIG_BOARDCTL_RESET)
} else if ((param1 == 1) && shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) { } else if ((param1 == 1) && shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) {
// 1: Reboot autopilot // 1: Reboot autopilot
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
while (1) { px4_usleep(1); } while (1) { px4_usleep(1); }
@@ -1004,7 +1000,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
} else if ((param1 == 2) && shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) { } else if ((param1 == 2) && shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) {
// 2: Shutdown autopilot // 2: Shutdown autopilot
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
while (1) { px4_usleep(1); } while (1) { px4_usleep(1); }
@@ -1014,14 +1010,14 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
} else if ((param1 == 3) && shutdown_if_allowed() && (px4_reboot_request(true, 400_ms) == 0)) { } else if ((param1 == 3) && shutdown_if_allowed() && (px4_reboot_request(true, 400_ms) == 0)) {
// 3: Reboot autopilot and keep it in the bootloader until upgraded. // 3: Reboot autopilot and keep it in the bootloader until upgraded.
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
while (1) { px4_usleep(1); } while (1) { px4_usleep(1); }
#endif // CONFIG_BOARDCTL_RESET #endif // CONFIG_BOARDCTL_RESET
} else { } else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
} }
} }
@@ -1033,7 +1029,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
|| status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN || _worker_thread.isBusy()) { || status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN || _worker_thread.isBusy()) {
// reject if armed or shutting down // reject if armed or shutting down
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
} else { } else {
@@ -1045,14 +1041,14 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
(cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL)) (cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL))
) { ) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
break; break;
} }
if ((int)(cmd.param1) == 1) { if ((int)(cmd.param1) == 1) {
/* gyro calibration */ /* gyro calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
status_flags.condition_calibration_enabled = true; status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::GyroCalibration); _worker_thread.startTask(WorkerThread::Request::GyroCalibration);
@@ -1064,62 +1060,62 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
} else if ((int)(cmd.param2) == 1) { } else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */ /* magnetometer calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
status_flags.condition_calibration_enabled = true; status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::MagCalibration); _worker_thread.startTask(WorkerThread::Request::MagCalibration);
} else if ((int)(cmd.param3) == 1) { } else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */ /* zero-altitude pressure calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
} else if ((int)(cmd.param4) == 1) { } else if ((int)(cmd.param4) == 1) {
/* RC calibration */ /* RC calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
/* disable RC control input completely */ /* disable RC control input completely */
status_flags.rc_input_blocked = true; status_flags.rc_input_blocked = true;
mavlink_log_info(&mavlink_log_pub, "Calibration: Disabling RC input"); mavlink_log_info(&mavlink_log_pub, "Calibration: Disabling RC input");
} else if ((int)(cmd.param4) == 2) { } else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */ /* RC trim calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
status_flags.condition_calibration_enabled = true; status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::RCTrimCalibration); _worker_thread.startTask(WorkerThread::Request::RCTrimCalibration);
} else if ((int)(cmd.param5) == 1) { } else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */ /* accelerometer calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
status_flags.condition_calibration_enabled = true; status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::AccelCalibration); _worker_thread.startTask(WorkerThread::Request::AccelCalibration);
} else if ((int)(cmd.param5) == 2) { } else if ((int)(cmd.param5) == 2) {
// board offset calibration // board offset calibration
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
status_flags.condition_calibration_enabled = true; status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::LevelCalibration); _worker_thread.startTask(WorkerThread::Request::LevelCalibration);
} else if ((int)(cmd.param5) == 4) { } else if ((int)(cmd.param5) == 4) {
// accelerometer quick calibration // accelerometer quick calibration
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
status_flags.condition_calibration_enabled = true; status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick); _worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick);
} else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) { } else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) {
// TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017) // TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017)
/* airspeed calibration */ /* airspeed calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
status_flags.condition_calibration_enabled = true; status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::AirspeedCalibration); _worker_thread.startTask(WorkerThread::Request::AirspeedCalibration);
} else if ((int)(cmd.param7) == 1) { } else if ((int)(cmd.param7) == 1) {
/* do esc calibration */ /* do esc calibration */
if (check_battery_disconnected(&mavlink_log_pub)) { if (check_battery_disconnected(&mavlink_log_pub)) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
status_flags.condition_calibration_enabled = true; status_flags.condition_calibration_enabled = true;
_armed.in_esc_calibration_mode = true; _armed.in_esc_calibration_mode = true;
_worker_thread.startTask(WorkerThread::Request::ESCCalibration); _worker_thread.startTask(WorkerThread::Request::ESCCalibration);
} else { } else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
} }
} else if ((int)(cmd.param4) == 0) { } else if ((int)(cmd.param4) == 0) {
@@ -1130,10 +1126,10 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
mavlink_log_info(&mavlink_log_pub, "Calibration: Restoring RC input"); mavlink_log_info(&mavlink_log_pub, "Calibration: Restoring RC input");
} }
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else { } else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
} }
} }
@@ -1147,10 +1143,10 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
|| _worker_thread.isBusy()) { || _worker_thread.isBusy()) {
// reject if armed or shutting down // reject if armed or shutting down
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
} else { } else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
// parameter 1: Yaw in degrees // parameter 1: Yaw in degrees
// parameter 3: Latitude // parameter 3: Latitude
// parameter 4: Longitude // parameter 4: Longitude
@@ -1184,20 +1180,20 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
|| _worker_thread.isBusy()) { || _worker_thread.isBusy()) {
// reject if armed or shutting down // reject if armed or shutting down
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
} else { } else {
if (((int)(cmd.param1)) == 0) { if (((int)(cmd.param1)) == 0) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
_worker_thread.startTask(WorkerThread::Request::ParamLoadDefault); _worker_thread.startTask(WorkerThread::Request::ParamLoadDefault);
} else if (((int)(cmd.param1)) == 1) { } else if (((int)(cmd.param1)) == 1) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
_worker_thread.startTask(WorkerThread::Request::ParamSaveDefault); _worker_thread.startTask(WorkerThread::Request::ParamSaveDefault);
} else if (((int)(cmd.param1)) == 2) { } else if (((int)(cmd.param1)) == 2) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
_worker_thread.startTask(WorkerThread::Request::ParamResetAll); _worker_thread.startTask(WorkerThread::Request::ParamResetAll);
} }
} }
@@ -1207,7 +1203,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
/* just ack, implementation handled in the IO driver */ /* just ack, implementation handled in the IO driver */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
break; break;
case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
@@ -1245,13 +1241,13 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
default: default:
/* Warn about unsupported commands, this makes sense because only commands /* Warn about unsupported commands, this makes sense because only commands
* to this component ID (or all) are passed by mavlink. */ * to this component ID (or all) are passed by mavlink. */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
break; break;
} }
if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */ /* already warned about unsupported commands in "default" case */
answer_command(cmd, cmd_result, command_ack_pub); answer_command(cmd, cmd_result);
} }
return true; return true;
@@ -2316,7 +2312,7 @@ Commander::run()
PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, _cmd_sub.get_last_generation()); PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, _cmd_sub.get_last_generation());
} }
if (handle_command(&status, cmd, &_armed, _command_ack_pub)) { if (handle_command(&status, cmd, &_armed)) {
_status_changed = true; _status_changed = true;
} }
} }
@@ -3495,8 +3491,7 @@ Commander::print_reject_arm(const char *msg)
} }
} }
void answer_command(const vehicle_command_s &cmd, unsigned result, void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
uORB::Publication<vehicle_command_ack_s> &command_ack_pub)
{ {
switch (result) { switch (result) {
case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
@@ -3524,15 +3519,12 @@ void answer_command(const vehicle_command_s &cmd, unsigned result,
/* publish ACK */ /* publish ACK */
vehicle_command_ack_s command_ack{}; vehicle_command_ack_s command_ack{};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command; command_ack.command = cmd.command;
command_ack.result = (uint8_t)result; command_ack.result = result;
command_ack.target_system = cmd.source_system; command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component; command_ack.target_component = cmd.source_component;
command_ack.timestamp = hrt_absolute_time();
_command_ack_pub.publish(command_ack);
command_ack_pub.publish(command_ack);
} }
int Commander::task_spawn(int argc, char *argv[]) int Commander::task_spawn(int argc, char *argv[])
src/modules/commander/Commander.hpp
+2 -2
View File
@@ -116,6 +116,7 @@ public:
void get_circuit_breaker_params(); void get_circuit_breaker_params();
private: private:
void answer_command(const vehicle_command_s &cmd, uint8_t result);
transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub,
arm_disarm_reason_t calling_reason); arm_disarm_reason_t calling_reason);
@@ -143,8 +144,7 @@ private:
void estimator_check(const vehicle_status_flags_s &status_flags); void estimator_check(const vehicle_status_flags_s &status_flags);
bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed, bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed);
uORB::Publication<vehicle_command_ack_s> &command_ack_pub);
unsigned handle_command_motor_test(const vehicle_command_s &cmd); unsigned handle_command_motor_test(const vehicle_command_s &cmd);