diff --git a/msg/vehicle_command_ack.msg b/msg/vehicle_command_ack.msg new file mode 100644 index 0000000000..2c90469c0e --- /dev/null +++ b/msg/vehicle_command_ack.msg @@ -0,0 +1,8 @@ +uint8 VEHICLE_RESULT_ACCEPTED = 0 +uint8 VEHICLE_RESULT_TEMPORARILY_REJECTED = 1 +uint8 VEHICLE_RESULT_DENIED = 2 +uint8 VEHICLE_RESULT_UNSUPPORTED = 3 +uint8 VEHICLE_RESULT_FAILED = 4 + +uint16 command +uint8 result diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 50a4372756..499b11b6c1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -94,6 +94,7 @@ #include #include #include +#include #include #include @@ -227,7 +228,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); + 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); /** * Mainloop of commander. @@ -270,7 +272,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & */ void *commander_low_prio_loop(void *arg); -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); /** * check whether autostart ID is in the reserved range for HIL setups @@ -543,7 +546,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_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 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) { /* 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) @@ -904,19 +908,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); + answer_command(*cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, *command_ack_pub, *command_ack); break; } if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, cmd_result); - } - - /* send any requested ACKs */ - if (cmd->confirmation > 0 && cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { - /* send acknowledge command */ - // XXX TODO + answer_command(*cmd, cmd_result, *command_ack_pub, *command_ack); } return true; @@ -1150,6 +1148,11 @@ int commander_thread_main(int argc, char *argv[]) orb_advert_t home_pub = nullptr; memset(&_home, 0, sizeof(_home)); + /* 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 */ orb_advert_t mission_pub = nullptr; mission_s mission; @@ -2354,7 +2357,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &attitude, &home_pub)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, + &attitude, &home_pub, &command_ack_pub, &command_ack)) { status_changed = true; } } @@ -3107,7 +3111,8 @@ 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) { switch (result) { case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: @@ -3138,6 +3143,17 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result) default: break; } + + /* publish ACK */ + command_ack.command = cmd.command; + command_ack.result = result; + + if (command_ack_pub != nullptr) { + orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack); + + } else { + command_ack_pub = orb_advertise(ORB_ID(vehicle_command_ack), &command_ack); + } } void *commander_low_prio_loop(void *arg) @@ -3150,6 +3166,11 @@ void *commander_low_prio_loop(void *arg) struct vehicle_command_s cmd; memset(&cmd, 0, sizeof(cmd)); + /* command ack */ + orb_advert_t command_ack_pub = nullptr; + struct vehicle_command_ack_s command_ack; + memset(&command_ack, 0, sizeof(command_ack)); + /* timeout for param autosave */ hrt_abstime need_param_autosave_timeout = 0; @@ -3207,23 +3228,23 @@ void *commander_low_prio_loop(void *arg) if (is_safe(&status, &safety, &armed)) { if (((int)(cmd.param1)) == 1) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); usleep(100000); /* reboot */ px4_systemreset(false); } else if (((int)(cmd.param1)) == 3) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); usleep(100000); /* reboot to bootloader */ px4_systemreset(true); } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); } } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); } break; @@ -3235,7 +3256,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, false /* fRunPreArmChecks */, mavlink_fd)) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); break; } else { status.calibration_enabled = true; @@ -3243,21 +3264,21 @@ void *commander_low_prio_loop(void *arg) if ((int)(cmd.param1) == 1) { /* gyro calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_gyro_calibration(mavlink_fd); } else if ((int)(cmd.param2) == 1) { /* magnetometer calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_mag_calibration(mavlink_fd); } else if ((int)(cmd.param3) == 1) { /* zero-altitude pressure calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); } else if ((int)(cmd.param4) == 1) { /* RC calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); /* disable RC control input completely */ status.rc_input_blocked = true; calib_ret = OK; @@ -3265,31 +3286,31 @@ 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); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_trim_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_accel_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 2) { // board offset calibration - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_level_calibration(mavlink_fd); } else if ((int)(cmd.param6) == 1) { /* airspeed calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_airspeed_calibration(mavlink_fd); } else if ((int)(cmd.param7) == 1) { /* do esc calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_esc_calibration(mavlink_fd, &armed); } else if ((int)(cmd.param4) == 0) { /* RC calibration ended - have we been in one worth confirming? */ if (status.rc_input_blocked) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); /* enable RC control input */ status.rc_input_blocked = false; mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); @@ -3336,7 +3357,7 @@ void *commander_low_prio_loop(void *arg) if (ret == OK) { mavlink_log_info(mavlink_fd, "settings loaded"); - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); } else { mavlink_log_critical(mavlink_fd, "settings load ERROR"); @@ -3350,7 +3371,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); } - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); } } else if (((int)(cmd.param1)) == 1) { @@ -3364,7 +3385,7 @@ void *commander_low_prio_loop(void *arg) } /* do not spam MAVLink, but provide the answer / green led mechanism */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); } else { mavlink_log_critical(mavlink_fd, "settings save error"); @@ -3378,7 +3399,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); } - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); } } else if (((int)(cmd.param1)) == 2) { @@ -3389,11 +3410,11 @@ void *commander_low_prio_loop(void *arg) if (ret == OK) { /* do not spam MAVLink, but provide the answer / green led mechanism */ mavlink_log_critical(mavlink_fd, "onboard parameters reset"); - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); } else { mavlink_log_critical(mavlink_fd, "param reset error"); - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); } } @@ -3408,13 +3429,6 @@ void *commander_low_prio_loop(void *arg) /* don't answer on unsupported commands, it will be done in main loop */ break; } - - /* send any requested ACKs */ - if (cmd.confirmation > 0 && cmd.command != vehicle_command_s::VEHICLE_CMD_DO_SET_MODE - && cmd.command != vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM) { - /* send acknowledge command */ - // XXX TODO - } } } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 07b768354f..8e575e1b8f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -77,6 +77,7 @@ #include #include +#include #include "mavlink_bridge_header.h" #include "mavlink_main.h" @@ -1639,9 +1640,13 @@ Mavlink::task_main(int argc, char *argv[]) uint64_t param_time = 0; 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; 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 */ @@ -1856,6 +1861,15 @@ Mavlink::task_main(int argc, char *argv[]) set_manual_input_mode_generation(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED); } + /* send command ACK */ + if (ack_sub->update(&ack_time, &command_ack)) { + mavlink_command_ack_t msg; + msg.result = command_ack.result; + msg.command = command_ack.command; + + send_message(MAVLINK_MSG_ID_COMMAND_ACK, &msg); + } + /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1b8fb18735..15dab60df4 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -270,3 +270,6 @@ ORB_DEFINE(distance_sensor, struct distance_sensor_s); #include "topics/camera_trigger.h" ORB_DEFINE(camera_trigger, struct camera_trigger_s); + +#include "topics/vehicle_command_ack.h" +ORB_DEFINE(vehicle_command_ack, struct vehicle_command_ack_s);