fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 03:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

gps injection: use the orb queuing API instead of multiple instances

Browse Source 
It uses a queue length of 6. There are 3 RTCM msgs/s, but due to
fragmentation and WiFi lags, there can be more than that. During several
tests, a length of 6 showed no queue overflows.
This commit is contained in:
Beat Küng
2016-08-02 09:06:59 +02:00
committed by Lorenz Meier
parent 9398a4819f
commit 124e1c26d9
3 changed files with 13 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+4 -9
View File
@@ -125,6 +125,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_time_offset_pub(nullptr),
_follow_target_pub(nullptr),
_transponder_report_pub(nullptr),
_gps_inject_data_pub(nullptr),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
_old_timestamp(0),
@@ -143,9 +144,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_mom_switch_pos{},
_mom_switch_state(0)
{
for (int i = 0; i < _gps_inject_data_pub_size; ++i) {
_gps_inject_data_pub[i] = nullptr;
}
}
MavlinkReceiver::~MavlinkReceiver()
@@ -1873,19 +1871,16 @@ void MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
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];
orb_advert_t &pub = _gps_inject_data_pub;
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);
pub = orb_advertise_queue(ORB_ID(gps_inject_data), &gps_inject_data_topic,
_gps_inject_data_queue_size);
} 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
src/modules/mavlink/mavlink_receiver.h
+2 -3
View File
@@ -218,9 +218,8 @@ private:
orb_advert_t _time_offset_pub;
orb_advert_t _follow_target_pub;
orb_advert_t _transponder_report_pub;
static const int _gps_inject_data_pub_size = 4;
orb_advert_t _gps_inject_data_pub[_gps_inject_data_pub_size];
int _gps_inject_data_next_idx = 0;
static const int _gps_inject_data_queue_size = 6;
orb_advert_t _gps_inject_data_pub;
int _control_mode_sub;
int _hil_frames;
uint64_t _old_timestamp;