fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-13 20:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

orb: add gps_inject_data message & publish from mavlink

Browse Source
This commit is contained in:
Beat Küng
2016-04-25 09:48:24 +02:00
committed by Lorenz Meier
parent faa85a2eba
commit 5cf351f585
4 changed files with 44 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+33 -1
View File
@@ -129,6 +129,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_time_offset_pub(nullptr),
_follow_target_pub(nullptr),
_transponder_report_pub(nullptr),
_gps_inject_data_pub(),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
_old_timestamp(0),
@@ -147,7 +148,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_mom_switch_pos{},
_mom_switch_state(0)
{
_gps_inject_data_pub.fill(nullptr);
}
MavlinkReceiver::~MavlinkReceiver()
@@ -237,6 +238,11 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
case MAVLINK_MSG_ID_ADSB_VEHICLE:
handle_message_adsb_vehicle(msg);
break;
case MAVLINK_MSG_ID_GPS_INJECT_DATA:
handle_message_gps_inject_data(msg);
break;
default:
break;
}
@@ -1686,6 +1692,32 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
}
}
void MavlinkReceiver::handle_message_gps_inject_data(mavlink_message_t *msg)
{
mavlink_gps_inject_data_t gps_inject_data_msg;
gps_inject_data_s gps_inject_data_topic;
mavlink_msg_gps_inject_data_decode(msg, &gps_inject_data_msg);
gps_inject_data_topic.len = gps_inject_data_msg.len;
memcpy(gps_inject_data_topic.data, gps_inject_data_msg.data,
sizeof(uint8_t) * math::min((uint8_t)110, gps_inject_data_msg.len));
orb_advert_t &pub = _gps_inject_data_pub[_gps_inject_data_next_idx];
if (pub == nullptr) {
int idx = _gps_inject_data_next_idx;
pub = orb_advertise_multi(ORB_ID(gps_inject_data), &gps_inject_data_topic,
&idx, ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(gps_inject_data), pub, &gps_inject_data_topic);
}
_gps_inject_data_next_idx = (_gps_inject_data_next_idx + 1) % _gps_inject_data_pub.size();
}
void
MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
{