forked from Archive/PX4-Autopilot
gps injection: use the orb queuing API instead of multiple instances
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:
parent
9398a4819f
commit
124e1c26d9
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue