From 89a428fbfe400889e34a2494c01f7ffeb862ab15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Fri, 23 Jun 2017 13:48:54 -0700 Subject: [PATCH] commander: Keep vehicle_command_ack_s local No need to keep this struct as global or alive while looping. --- src/modules/commander/commander.cpp | 68 +++++++++++++--------------- src/modules/mavlink/mavlink_main.cpp | 5 +- 2 files changed, 34 insertions(+), 39 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fbcb7a28bc..88cd7d8a4f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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, 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, - orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack, struct vehicle_roi_s *roi, - orb_advert_t *roi_pub, - bool *changed); + orb_advert_t *command_ack_pub, struct vehicle_roi_s *roi, + orb_advert_t *roi_pub, bool *changed); /** * 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); 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*/ 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 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, - orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack, - struct vehicle_roi_s *roi, orb_advert_t *roi_pub, bool *changed) + orb_advert_t *command_ack_pub, struct vehicle_roi_s *roi, + orb_advert_t *roi_pub, bool *changed) { /* 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) @@ -1233,13 +1232,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s default: /* Warn about unsupported commands, this makes sense because only commands * 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; } if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { /* 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; @@ -1524,8 +1523,6 @@ int commander_thread_main(int argc, char *argv[]) /* command ack */ 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 */ mission_s mission; @@ -2994,7 +2991,7 @@ int commander_thread_main(int argc, char *argv[]) /* handle it */ 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; } } @@ -4100,7 +4097,7 @@ print_reject_arm(const char *msg) } 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) { case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: @@ -4128,6 +4125,7 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result, } /* publish ACK */ + vehicle_command_ack_s command_ack = {}; command_ack.command = cmd.command; command_ack.result = result; @@ -4151,8 +4149,6 @@ void *commander_low_prio_loop(void *arg) /* command ack */ orb_advert_t command_ack_pub = nullptr; - struct vehicle_command_ack_s command_ack; - memset(&command_ack, 0, sizeof(command_ack)); /* wakeup source(s) */ px4_pollfd_struct_t fds[1]; @@ -4191,23 +4187,23 @@ void *commander_low_prio_loop(void *arg) if (is_safe(&safety, &armed)) { 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); /* reboot */ px4_shutdown_request(true, false); } 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); /* reboot to bootloader */ px4_shutdown_request(true, true); } 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 { - 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; @@ -4230,7 +4226,7 @@ void *commander_low_prio_loop(void *arg) arm_mission_required, 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; } else { status_flags.condition_calibration_enabled = true; @@ -4238,7 +4234,7 @@ void *commander_low_prio_loop(void *arg) if ((int)(cmd.param1) == 1) { /* 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); } 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) { /* 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); } else if ((int)(cmd.param3) == 1) { /* 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) { /* 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 */ status_flags.rc_input_blocked = true; calib_ret = OK; @@ -4266,26 +4262,26 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 2) { /* 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); } else if ((int)(cmd.param5) == 1) { /* 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); } else if ((int)(cmd.param5) == 2) { // 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); } 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) /* 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); } else if ((int)(cmd.param7) == 1) { /* 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); } else if ((int)(cmd.param4) == 0) { @@ -4295,11 +4291,11 @@ void *commander_low_prio_loop(void *arg) status_flags.rc_input_blocked = false; 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 */ calib_ret = OK; } 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; @@ -4352,7 +4348,7 @@ void *commander_low_prio_loop(void *arg) if (ret == OK) { 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 { 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)); } - 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) { @@ -4381,7 +4377,7 @@ void *commander_low_prio_loop(void *arg) if (ret == OK) { /* 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 { 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)); } - 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) { @@ -4404,7 +4400,7 @@ void *commander_low_prio_loop(void *arg) /* do not spam MAVLink, but provide the answer / green led mechanism */ 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; @@ -4412,7 +4408,7 @@ void *commander_low_prio_loop(void *arg) case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: /* 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; default: diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6c6ed0a269..ba47151d1c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1996,17 +1996,15 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); uint64_t status_time = 0; 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 * beginning and not just when the topic exists. */ ack_sub->subscribe_from_beginning(true); - uint64_t ack_time = 0; MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log)); struct vehicle_status_s 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 */ @@ -2202,6 +2200,7 @@ Mavlink::task_main(int argc, char *argv[]) /* send command ACK */ uint16_t current_command_ack = 0; + struct vehicle_command_ack_s command_ack = {}; if (ack_sub->update(&ack_time, &command_ack)) { mavlink_command_ack_t msg;