fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 00:03:33 +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-06 20:41:38 +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/drivers/gps/gps.cpp
+7 -20
View File
@@ -138,9 +138,7 @@ private:
bool _fake_gps; ///< fake gps output
int _gps_num; ///< number of GPS connected
static const int _orb_inject_data_fd_count = 4;
int _orb_inject_data_fd[_orb_inject_data_fd_count];
int _orb_inject_data_next = 0;
int _orb_inject_data_fd;
orb_advert_t _dump_communication_pub; ///< if non-null, dump communication
gps_dump_s *_dump_to_device;
@@ -259,6 +257,7 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num
_last_rate_rtcm_injection_count(0),
_fake_gps(fake_gps),
_gps_num(gps_num),
_orb_inject_data_fd(-1),
_dump_communication_pub(nullptr),
_dump_to_device(nullptr),
_dump_from_device(nullptr)
@@ -276,10 +275,6 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num
_p_report_sat_info = &_sat_info->_data;
memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
}
for (int i = 0; i < _orb_inject_data_fd_count; ++i) {
_orb_inject_data_fd[i] = -1;
}
}
GPS::~GPS()
@@ -426,19 +421,18 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
void GPS::handleInjectDataTopic()
{
if (_orb_inject_data_fd[0] == -1) {
if (_orb_inject_data_fd == -1) {
return;
}
bool updated = false;
do {
int orb_inject_data_cur_fd = _orb_inject_data_fd[_orb_inject_data_next];
orb_check(orb_inject_data_cur_fd, &updated);
orb_check(_orb_inject_data_fd, &updated);
if (updated) {
struct gps_inject_data_s msg;
orb_copy(ORB_ID(gps_inject_data), orb_inject_data_cur_fd, &msg);
orb_copy(ORB_ID(gps_inject_data), _orb_inject_data_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
@@ -446,8 +440,6 @@ void GPS::handleInjectDataTopic()
*/
injectData(msg.data, msg.len);
_orb_inject_data_next = (_orb_inject_data_next + 1) % _orb_inject_data_fd_count;
++_last_rate_rtcm_injection_count;
}
} while (updated);
@@ -658,9 +650,7 @@ GPS::task_main()
fcntl(_serial_fd, F_SETFL, flags | O_NONBLOCK);
#endif
for (int i = 0; i < _orb_inject_data_fd_count; ++i) {
_orb_inject_data_fd[i] = orb_subscribe_multi(ORB_ID(gps_inject_data), i);
}
_orb_inject_data_fd = orb_subscribe(ORB_ID(gps_inject_data));
initializeCommunicationDump();
@@ -834,10 +824,7 @@ GPS::task_main()
PX4_INFO("exiting");
for (size_t i = 0; i < _orb_inject_data_fd_count; ++i) {
orb_unsubscribe(_orb_inject_data_fd[i]);
_orb_inject_data_fd[i] = -1;
}
orb_unsubscribe(_orb_inject_data_fd);
if (_dump_communication_pub) {
orb_unadvertise(_dump_communication_pub);