fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
src/modules/commander/commander.cpp
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,
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:

5
src/modules/mavlink/mavlink_main.cpp
View File

@ -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;