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

implemented command ACK

Browse Source
This commit is contained in:
Andreas Antener 2015-12-12 13:01:49 +01:00 committed by Lorenz Meier
parent 94bfad5057
commit e8e81650dc
4 changed files with 80 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
msg/vehicle_command_ack.msg Normal file
View File

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

96
src/modules/commander/commander.cpp
View File

@ -94,6 +94,7 @@
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@ -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
}
}
}

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

@ -77,6 +77,7 @@
#include <mavlink/mavlink_log.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_command_ack.h>
#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)) {

3
src/modules/uORB/objects_common.cpp
View File

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