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

Add support for incoming NAMED_VALUE_FLOAT mavlink messages

This commit is contained in:
Julien Lecoeur 2017-06-07 15:32:46 +02:00 committed by Nuno Marques
parent 595d706eaf
commit b6c30cf9b2
2 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -137,6 +137,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_transponder_report_pub(nullptr),
_collision_report_pub(nullptr),
_control_state_pub(nullptr),
_debug_key_value_pub(nullptr),
_gps_inject_data_pub(nullptr),
_command_ack_pub(nullptr),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
@ -307,6 +308,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_play_tune(msg);
break;
case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
handle_message_named_value_float(msg);
break;
default:
break;
}
@ -2358,6 +2363,27 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
}
}
void MavlinkReceiver::handle_message_named_value_float(mavlink_message_t *msg)
{
mavlink_named_value_float_t debug_msg;
debug_key_value_s debug_topic = {};
mavlink_msg_named_value_float_decode(msg, &debug_msg);
debug_topic.timestamp = hrt_absolute_time();
debug_topic.timestamp_ms = debug_msg.time_boot_ms;
memcpy(debug_topic.key, debug_msg.name, sizeof(debug_topic.key));
debug_topic.key[sizeof(debug_topic.key) - 1] = '\0'; // enforce null termination
debug_topic.value = debug_msg.value;
if (_debug_key_value_pub == nullptr) {
_debug_key_value_pub = orb_advertise(ORB_ID(debug_key_value), &debug_topic);
} else {
orb_publish(ORB_ID(debug_key_value), _debug_key_value_pub, &debug_topic);
}
}
/**
* Receive data from UART.
*/

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

@ -151,6 +151,7 @@ private:
void handle_message_serial_control(mavlink_message_t *msg);
void handle_message_logging_ack(mavlink_message_t *msg);
void handle_message_play_tune(mavlink_message_t *msg);
void handle_message_named_value_float(mavlink_message_t *msg);
void *receive_thread(void *arg);
@ -236,6 +237,7 @@ private:
orb_advert_t _transponder_report_pub;
orb_advert_t _collision_report_pub;
orb_advert_t _control_state_pub;
orb_advert_t _debug_key_value_pub;
static const int _gps_inject_data_queue_size = 6;
orb_advert_t _gps_inject_data_pub;
orb_advert_t _command_ack_pub;