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

mavlink: Move request message logic to function

This commit is contained in:
Julian Oes 2020-05-28 10:52:16 +02:00
parent 62bf0a6cf8
commit 27b6c209e0
2 changed files with 40 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -430,34 +430,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
get_message_interval((int)roundf(cmd_mavlink.param1));
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) {
uint16_t message_id = (uint16_t)roundf(cmd_mavlink.param1);
bool stream_found = false;
for (const auto stream : _mavlink->get_streams()) {
if (stream->get_id() == message_id) {
stream_found = stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
break;
}
}
if (!stream_found) {
MavlinkStream *stream = create_mavlink_stream(message_id, _mavlink);
if (stream == nullptr) {
result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
} else {
bool message_sent = stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
if (!message_sent) {
result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
}
delete stream;
}
}
result = handle_request_message_command(vehicle_command);
} else if (cmd_mavlink.command == MAV_CMD_SET_CAMERA_ZOOM) {
struct actuator_controls_s actuator_controls = {};
@ -521,6 +494,43 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
}
}
uint8_t MavlinkReceiver::handle_request_message_command(const vehicle_command_s &vehicle_command)
{
uint16_t message_id = (uint16_t)roundf(vehicle_command.param1);
bool stream_found = false;
uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
for (const auto stream : _mavlink->get_streams()) {
if (stream->get_id() == message_id) {
stream_found = stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
break;
}
}
if (!stream_found) {
MavlinkStream *stream = create_mavlink_stream(message_id, _mavlink);
if (stream == nullptr) {
result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
} else {
bool message_sent = stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
if (!message_sent) {
result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
}
delete stream;
}
}
return result;
}
void
MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
{

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

@ -129,6 +129,8 @@ private:
void handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
const vehicle_command_s &vehicle_command);
uint8_t handle_request_message_command(const vehicle_command_s &vehicle_command);
void handle_message(mavlink_message_t *msg);
void handle_message_adsb_vehicle(mavlink_message_t *msg);