Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Handle reboot and shutdown commands. #8000

Merged
merged 1 commit into from Sep 23, 2017
Merged

Handle reboot and shutdown commands. #8000

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
6 changes: 6 additions & 0 deletions src/modules/commander/commander.cpp
View file
Open in desktop
Expand Up @@ -4217,6 +4217,12 @@ void *commander_low_prio_loop(void *arg)
/* reboot */
px4_shutdown_request(true, false);

} else if (((int)(cmd.param1)) == 2) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
usleep(100000);
/* shutdown */
px4_shutdown_request(false, false);

} else if (((int)(cmd.param1)) == 3) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
usleep(100000);
Expand Down
93 changes: 42 additions & 51 deletions src/modules/mavlink/mavlink_receiver.cpp
View file
Open in desktop
Expand Up @@ -170,6 +170,28 @@ MavlinkReceiver::~MavlinkReceiver()
orb_unsubscribe(_control_mode_sub);
}

void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, int ret)
{
vehicle_command_ack_s command_ack = {
.timestamp = 0,
.result_param2 = 0,
.command = command,
.result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED),
.from_external = false,
.result_param1 = 0,
.target_system = sysid,
.target_component = compid
};

if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);

} else {
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
}
}

void
MavlinkReceiver::handle_message(mavlink_message_t *msg)
{
Expand Down Expand Up @@ -432,11 +454,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);

bool send_ack = true;
int ret = 0;
int ret = PX4_OK;

if (!target_ok) {
ret = PX4_ERROR;
goto out;
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, ret);
return;
}

if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
Expand Down Expand Up @@ -509,27 +532,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
}
}

out:

if (send_ack) {
vehicle_command_ack_s command_ack = {
.timestamp = 0,
.result_param2 = 0,
.command = cmd_mavlink.command,
.result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED),
.from_external = false,
.result_param1 = 0,
.target_system = msg->sysid,
.target_component = msg->compid
};

if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);

} else {
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
}
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, ret);
}
}

Expand All @@ -543,22 +547,28 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);

bool send_ack = true;
int ret = 0;
int ret = PX4_OK;

if (!target_ok) {
ret = PX4_ERROR;
goto out;
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, ret);
return;
}

//check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 10) {
/* This is the link shutdown command, terminate mavlink */
PX4_WARN("terminated by remote");
fflush(stdout);
usleep(50000);
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN) {

int cmd_id = int(cmd_mavlink.param1);

if (cmd_id == 10) {
/* This is the link shutdown command, terminate mavlink */
PX4_WARN("terminated by remote");
fflush(stdout);
usleep(50000);

/* terminate other threads and this thread */
_mavlink->_task_should_exit = true;
/* terminate other threads and this thread */
_mavlink->_task_should_exit = true;
}

} else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
/* send autopilot version message */
Expand Down Expand Up @@ -606,27 +616,8 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
}
}

out:

if (send_ack) {
vehicle_command_ack_s command_ack = {
.timestamp = 0,
.result_param2 = 0,
.command = cmd_mavlink.command,
.result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED),
.from_external = false,
.result_param1 = 0,
.target_system = msg->sysid,
.target_component = msg->compid
};

if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);

} else {
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
}
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, ret);
}
}

Expand Down
1 change: 1 addition & 0 deletions src/modules/mavlink/mavlink_receiver.h
View file
Open in desktop
Expand Up @@ -117,6 +117,7 @@ class MavlinkReceiver

private:

void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, int ret);
void handle_message(mavlink_message_t *msg);
void handle_message_command_long(mavlink_message_t *msg);
void handle_message_command_int(mavlink_message_t *msg);
Expand Down