Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

commander: Keep vehicle_command_ack_s local

Browse Source 
No need to keep this struct as global or alive while looping.
This commit is contained in:
José Roberto de Souza
2017-06-23 13:48:54 -07:00
committed by Lorenz Meier
parent 7082cc13e0
commit 89a428fbfe
2 changed files with 34 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+32 -36
View File
@@ -281,9 +281,8 @@ void usage(const char *reason);
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd,
struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub, struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub,
orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack, struct vehicle_roi_s *roi, orb_advert_t *command_ack_pub, struct vehicle_roi_s *roi,
orb_advert_t *roi_pub, orb_advert_t *roi_pub, bool *changed);
bool *changed);
/** /**
* Mainloop of commander. * Mainloop of commander.
@@ -329,7 +328,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
void *commander_low_prio_loop(void *arg); void *commander_low_prio_loop(void *arg);
static void answer_command(struct vehicle_command_s &cmd, unsigned result, static void answer_command(struct vehicle_command_s &cmd, unsigned result,
orb_advert_t &command_ack_pub, vehicle_command_ack_s &command_ack); orb_advert_t &command_ack_pub);
/* publish vehicle status flags from the global variable status_flags*/ /* publish vehicle status flags from the global variable status_flags*/
static void publish_status_flags(orb_advert_t &vehicle_status_flags_pub); static void publish_status_flags(orb_advert_t &vehicle_status_flags_pub);
@@ -753,8 +752,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
struct home_position_s *home, struct vehicle_global_position_s *global_pos, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub, struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub,
orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack, orb_advert_t *command_ack_pub, struct vehicle_roi_s *roi,
struct vehicle_roi_s *roi, orb_advert_t *roi_pub, bool *changed) orb_advert_t *roi_pub, bool *changed)
{ {
/* 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)
@@ -1233,13 +1232,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
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, *command_ack); answer_command(*cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, *command_ack_pub);
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, *command_ack); answer_command(*cmd, cmd_result, *command_ack_pub);
} }
return true; return true;
@@ -1524,8 +1523,6 @@ int commander_thread_main(int argc, char *argv[])
/* command ack */ /* command ack */
orb_advert_t command_ack_pub = nullptr; orb_advert_t command_ack_pub = nullptr;
struct vehicle_command_ack_s command_ack;
memset(&command_ack, 0, sizeof(command_ack));
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
mission_s mission; mission_s mission;
@@ -2994,7 +2991,7 @@ int commander_thread_main(int argc, char *argv[])
/* handle it */ /* handle it */
if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position,
&attitude, &home_pub, &command_ack_pub, &command_ack, &_roi, &roi_pub, &status_changed)) { &attitude, &home_pub, &command_ack_pub, &_roi, &roi_pub, &status_changed)) {
status_changed = true; status_changed = true;
} }
} }
@@ -4100,7 +4097,7 @@ print_reject_arm(const char *msg)
} }
void answer_command(struct vehicle_command_s &cmd, unsigned result, void answer_command(struct vehicle_command_s &cmd, unsigned result,
orb_advert_t &command_ack_pub, vehicle_command_ack_s &command_ack) orb_advert_t &command_ack_pub)
{ {
switch (result) { switch (result) {
case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
@@ -4128,6 +4125,7 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result,
} }
/* publish ACK */ /* publish ACK */
vehicle_command_ack_s command_ack = {};
command_ack.command = cmd.command; command_ack.command = cmd.command;
command_ack.result = result; command_ack.result = result;
@@ -4151,8 +4149,6 @@ void *commander_low_prio_loop(void *arg)
/* command ack */ /* command ack */
orb_advert_t command_ack_pub = nullptr; orb_advert_t command_ack_pub = nullptr;
struct vehicle_command_ack_s command_ack;
memset(&command_ack, 0, sizeof(command_ack));
/* wakeup source(s) */ /* wakeup source(s) */
px4_pollfd_struct_t fds[1]; px4_pollfd_struct_t fds[1];
@@ -4191,23 +4187,23 @@ void *commander_low_prio_loop(void *arg)
if (is_safe(&safety, &armed)) { if (is_safe(&safety, &armed)) {
if (((int)(cmd.param1)) == 1) { if (((int)(cmd.param1)) == 1) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
usleep(100000); usleep(100000);
/* reboot */ /* reboot */
px4_shutdown_request(true, false); px4_shutdown_request(true, false);
} else if (((int)(cmd.param1)) == 3) { } else if (((int)(cmd.param1)) == 3) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
usleep(100000); usleep(100000);
/* reboot to bootloader */ /* reboot to bootloader */
px4_shutdown_request(true, true); px4_shutdown_request(true, true);
} else { } else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
} }
} else { } else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
} }
break; break;
@@ -4230,7 +4226,7 @@ void *commander_low_prio_loop(void *arg)
arm_mission_required, arm_mission_required,
hrt_elapsed_time(&commander_boot_timestamp))) { hrt_elapsed_time(&commander_boot_timestamp))) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
break; break;
} else { } else {
status_flags.condition_calibration_enabled = true; status_flags.condition_calibration_enabled = true;
@@ -4238,7 +4234,7 @@ void *commander_low_prio_loop(void *arg)
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, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_gyro_calibration(&mavlink_log_pub); calib_ret = do_gyro_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || } else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
@@ -4249,16 +4245,16 @@ void *commander_low_prio_loop(void *arg)
} 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, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_mag_calibration(&mavlink_log_pub); calib_ret = do_mag_calibration(&mavlink_log_pub);
} 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, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
} 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, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
/* disable RC control input completely */ /* disable RC control input completely */
status_flags.rc_input_blocked = true; status_flags.rc_input_blocked = true;
calib_ret = OK; calib_ret = OK;
@@ -4266,26 +4262,26 @@ void *commander_low_prio_loop(void *arg)
} 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, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_trim_calibration(&mavlink_log_pub); calib_ret = do_trim_calibration(&mavlink_log_pub);
} 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, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_accel_calibration(&mavlink_log_pub); calib_ret = do_accel_calibration(&mavlink_log_pub);
} 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, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_level_calibration(&mavlink_log_pub); calib_ret = do_level_calibration(&mavlink_log_pub);
} 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, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_airspeed_calibration(&mavlink_log_pub); calib_ret = do_airspeed_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param7) == 1) { } else if ((int)(cmd.param7) == 1) {
/* do esc calibration */ /* do esc calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_esc_calibration(&mavlink_log_pub, &armed); calib_ret = do_esc_calibration(&mavlink_log_pub, &armed);
} else if ((int)(cmd.param4) == 0) { } else if ((int)(cmd.param4) == 0) {
@@ -4295,11 +4291,11 @@ void *commander_low_prio_loop(void *arg)
status_flags.rc_input_blocked = false; status_flags.rc_input_blocked = false;
mavlink_log_info(&mavlink_log_pub, "CAL: Re-enabling RC IN"); mavlink_log_info(&mavlink_log_pub, "CAL: Re-enabling RC IN");
} }
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
/* this always succeeds */ /* this always succeeds */
calib_ret = OK; calib_ret = OK;
} else { } else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub);
} }
status_flags.condition_calibration_enabled = false; status_flags.condition_calibration_enabled = false;
@@ -4352,7 +4348,7 @@ void *commander_low_prio_loop(void *arg)
if (ret == OK) { if (ret == OK) {
mavlink_log_info(&mavlink_log_pub, "settings loaded"); mavlink_log_info(&mavlink_log_pub, "settings loaded");
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
} else { } else {
mavlink_log_critical(&mavlink_log_pub, "settings load ERROR"); mavlink_log_critical(&mavlink_log_pub, "settings load ERROR");
@@ -4366,7 +4362,7 @@ void *commander_low_prio_loop(void *arg)
mavlink_log_critical(&mavlink_log_pub, "ERROR: %s", strerror(ret)); mavlink_log_critical(&mavlink_log_pub, "ERROR: %s", strerror(ret));
} }
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub);
} }
} else if (((int)(cmd.param1)) == 1) { } else if (((int)(cmd.param1)) == 1) {
@@ -4381,7 +4377,7 @@ void *commander_low_prio_loop(void *arg)
if (ret == OK) { if (ret == OK) {
/* do not spam MAVLink, but provide the answer / green led mechanism */ /* do not spam MAVLink, but provide the answer / green led mechanism */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
} else { } else {
mavlink_log_critical(&mavlink_log_pub, "settings save error"); mavlink_log_critical(&mavlink_log_pub, "settings save error");
@@ -4395,7 +4391,7 @@ void *commander_low_prio_loop(void *arg)
mavlink_log_critical(&mavlink_log_pub, "ERROR: %s", strerror(ret)); mavlink_log_critical(&mavlink_log_pub, "ERROR: %s", strerror(ret));
} }
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub);
} }
} else if (((int)(cmd.param1)) == 2) { } else if (((int)(cmd.param1)) == 2) {
@@ -4404,7 +4400,7 @@ void *commander_low_prio_loop(void *arg)
/* do not spam MAVLink, but provide the answer / green led mechanism */ /* do not spam MAVLink, but provide the answer / green led mechanism */
mavlink_log_critical(&mavlink_log_pub, "onboard parameters reset"); mavlink_log_critical(&mavlink_log_pub, "onboard parameters reset");
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
} }
break; break;
@@ -4412,7 +4408,7 @@ void *commander_low_prio_loop(void *arg)
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, command_ack); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
break; break;
default: default:
src/modules/mavlink/mavlink_main.cpp
+2 -3
View File
@@ -1996,17 +1996,15 @@ Mavlink::task_main(int argc, char *argv[])
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
uint64_t status_time = 0; uint64_t status_time = 0;
MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack)); MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack));
uint64_t ack_time = 0;
/* We don't want to miss the first advertise of an ACK, so we subscribe from the /* We don't want to miss the first advertise of an ACK, so we subscribe from the
* beginning and not just when the topic exists. */ * beginning and not just when the topic exists. */
ack_sub->subscribe_from_beginning(true); ack_sub->subscribe_from_beginning(true);
uint64_t ack_time = 0;
MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log)); MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
struct vehicle_status_s status; struct vehicle_status_s status;
status_sub->update(&status_time, &status); status_sub->update(&status_time, &status);
struct vehicle_command_ack_s command_ack;
ack_sub->update(&ack_time, &command_ack);
/* add default streams depending on mode */ /* add default streams depending on mode */
@@ -2202,6 +2200,7 @@ Mavlink::task_main(int argc, char *argv[])
/* send command ACK */ /* send command ACK */
uint16_t current_command_ack = 0; uint16_t current_command_ack = 0;
struct vehicle_command_ack_s command_ack = {};
if (ack_sub->update(&ack_time, &command_ack)) { if (ack_sub->update(&ack_time, &command_ack)) {
mavlink_command_ack_t msg; mavlink_command_ack_t msg;