Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 03:36:07 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

mavlink: Move request message logic to function

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+38 -28
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)); get_message_interval((int)roundf(cmd_mavlink.param1));
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) { } else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) {
uint16_t message_id = (uint16_t)roundf(cmd_mavlink.param1); result = handle_request_message_command(vehicle_command);
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;
}
}
} else if (cmd_mavlink.command == MAV_CMD_SET_CAMERA_ZOOM) { } else if (cmd_mavlink.command == MAV_CMD_SET_CAMERA_ZOOM) {
struct actuator_controls_s actuator_controls = {}; 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 void
MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
{ {
src/modules/mavlink/mavlink_receiver.h
+2
View File
@@ -129,6 +129,8 @@ private:
void handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink, void handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
const vehicle_command_s &vehicle_command); 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(mavlink_message_t *msg);
void handle_message_adsb_vehicle(mavlink_message_t *msg); void handle_message_adsb_vehicle(mavlink_message_t *msg);