dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink_receiver: directly pass the result to acknowledge() 

And use denied in case ulog streaming fails.
This commit is contained in:
Beat Küng 2017-10-31 10:37:59 +01:00 committed by Lorenz Meier
parent a2db639289
commit 459a71a6f1
2 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -169,13 +169,13 @@ MavlinkReceiver::~MavlinkReceiver()
orb_unsubscribe(_actuator_armed_sub);
}
void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, int ret)
void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result)
{
vehicle_command_ack_s command_ack = {
.timestamp = hrt_absolute_time(),
.result_param2 = 0,
.command = command,
.result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED),
.result = result,
.from_external = false,
.result_param1 = 0,
.target_system = sysid,
@ -509,11 +509,10 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
bool send_ack = true;
int ret = PX4_OK;
uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
if (!target_ok) {
ret = PX4_ERROR;
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, ret);
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_FAILED);
return;
}
@ -529,7 +528,9 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
} else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
ret = set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3);
if (set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3)) {
result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
}
} else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
get_message_interval((int)cmd_mavlink.param1);
@ -552,7 +553,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
// on a radio link
if (_mavlink->get_data_rate() < 5000) {
send_ack = true;
ret = PX4_ERROR;
result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
_mavlink->send_statustext_critical("Not enough bandwidth to enable log streaming");
} else {
@ -578,7 +579,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
}
if (send_ack) {
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, ret);
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, result);
}
}

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -115,7 +115,7 @@ public:
private:
void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, int ret);
void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result);
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);