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

RTCM: use MAVLINK_MSG_ID_GPS_RTCM_DATA mavlink message (supports fragmentation)

This commit is contained in:
Beat Küng 2016-05-24 10:00:59 +02:00 committed by Lorenz Meier
parent 0e3d660ccd
commit 25cff52019
4 changed files with 17 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/gps_inject_data.msg
View File

@ -1,2 +1,3 @@
uint8 len # length of data
uint8[110] data # data to write to GPS device
uint8 flags # LSB: 1=fragmented
uint8[182] data # data to write to GPS device (RTCM message)

5
src/drivers/gps/gps.cpp
View File

@ -441,6 +441,11 @@ void GPS::handleInjectDataTopic()
if (updated) {
struct gps_inject_data_s msg;
orb_copy(ORB_ID(gps_inject_data), orb_inject_data_cur_fd, &msg);
/* Write the message to the gps device. Note that the message could be fragmented.
* But as we don't write anywhere else to the device during operation, we don't
* need to assemble the message first.
*/
injectData(msg.data, msg.len);
_orb_inject_data_next = (_orb_inject_data_next + 1) % _orb_inject_data_fd_count;

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

@ -250,8 +250,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_adsb_vehicle(msg);
break;
case MAVLINK_MSG_ID_GPS_INJECT_DATA:
handle_message_gps_inject_data(msg);
case MAVLINK_MSG_ID_GPS_RTCM_DATA:
handle_message_gps_rtcm_data(msg);
break;
default:
@ -1775,16 +1775,17 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
}
}
void MavlinkReceiver::handle_message_gps_inject_data(mavlink_message_t *msg)
void MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
{
mavlink_gps_inject_data_t gps_inject_data_msg;
mavlink_gps_rtcm_data_t gps_rtcm_data_msg;
gps_inject_data_s gps_inject_data_topic;
mavlink_msg_gps_inject_data_decode(msg, &gps_inject_data_msg);
mavlink_msg_gps_rtcm_data_decode(msg, &gps_rtcm_data_msg);
gps_inject_data_topic.len = gps_inject_data_msg.len;
memcpy(gps_inject_data_topic.data, gps_inject_data_msg.data,
math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_inject_data_msg.len));
gps_inject_data_topic.len = gps_rtcm_data_msg.len;
gps_inject_data_topic.flags = gps_rtcm_data_msg.flags;
memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data,
math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_rtcm_data_msg.len));
orb_advert_t &pub = _gps_inject_data_pub[_gps_inject_data_next_idx];

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

@ -141,7 +141,7 @@ private:
void handle_message_distance_sensor(mavlink_message_t *msg);
void handle_message_follow_target(mavlink_message_t *msg);
void handle_message_adsb_vehicle(mavlink_message_t *msg);
void handle_message_gps_inject_data(mavlink_message_t *msg);
void handle_message_gps_rtcm_data(mavlink_message_t *msg);
void *receive_thread(void *arg);