fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 19:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added support for MAV_CMD_REQUEST_MESSAGE for mavlink streams

Browse Source
This commit is contained in:
Timothy Scott
2019-12-16 06:45:53 -08:00
committed by Julian Oes
parent 4bcdf560be
commit 7abadabc19
2 changed files with 30 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+23 -5
View File
@@ -47,6 +47,7 @@
#include <drivers/drv_tone_alarm.h>
#include <ecl/geo/geo.h>
#include <systemlib/px4_macros.h>
#include <math.h>
#ifdef CONFIG_NET
#include <net/if.h>
@@ -453,7 +454,6 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
const vehicle_command_s &vehicle_command)
{
bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
bool send_ack = true;
uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
@@ -474,19 +474,37 @@ 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) {
if (set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3)) {
if (set_message_interval((int)roundf(cmd_mavlink.param1), 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);
get_message_interval((int)roundf(cmd_mavlink.param1));
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_FLIGHT_INFORMATION) {
send_flight_information();
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_STORAGE_INFORMATION) {
if ((int)(cmd_mavlink.param2 + 0.5f) == 1) {
send_storage_information(cmd_mavlink.param1 + 0.5f);
if ((int)roundf(cmd_mavlink.param1) == 1) {
send_storage_information((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->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
stream_found = true;
break;
}
}
// TODO: Handle the case where a message is requested which could be sent, but for which there is no stream.
if (!stream_found) {
result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
}
} else {
src/modules/mavlink/mavlink_stream.h
+7
View File
@@ -92,6 +92,13 @@ public:
*/
virtual unsigned get_size() = 0;
/**
* This function is called in response to a MAV_CMD_REQUEST_MESSAGE command. The default implementation is to
* just reset the counter to immediately send one message.
*/
virtual void request_message(float param1 = 0.0, float param2 = 0.0, float param3 = 0.0, float param4 = 0.0,
float param5 = 0.0, float param6 = 0.0, float param7 = 0.0) { reset_last_sent(); }
/**
* Get the average message size
*