diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 0e079bbaca..b99406034b 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -124,8 +124,8 @@ private: GPSHelper *_helper; ///< instance of GPS parser GPS_Sat_Info *_sat_info; ///< instance of GPS sat info data object struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position - orb_advert_t _report_gps_pos_pub[2]; ///< uORB pub for gps position - int _gps_orb_instance[2]; ///< uORB multi-topic instance + orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position + int _gps_orb_instance; ///< uORB multi-topic instance struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate @@ -226,8 +226,8 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num _mode(GPS_DRIVER_MODE_UBX), _helper(nullptr), _sat_info(nullptr), - _report_gps_pos_pub{ nullptr, nullptr}, - _gps_orb_instance{ -1, -1}, + _report_gps_pos_pub{nullptr}, + _gps_orb_instance(-1), _p_report_sat_info(nullptr), _report_sat_info_pub(nullptr), _rate(0.0f), @@ -803,24 +803,7 @@ GPS::print_info() void GPS::publish() { - if (_gps_num == 1) { - if (_report_gps_pos_pub[0] != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[0], &_report_gps_pos); - - } else { - _report_gps_pos_pub[0] = orb_advertise_multi(ORB_ID(vehicle_gps_position), &_report_gps_pos, &_gps_orb_instance[0], ORB_PRIO_DEFAULT); - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[0], &_report_gps_pos); - } - - } else { - if (_report_gps_pos_pub[1] != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[1], &_report_gps_pos); - - } else { - _report_gps_pos_pub[1] = orb_advertise_multi(ORB_ID(vehicle_gps_position), &_report_gps_pos, &_gps_orb_instance[1], ORB_PRIO_DEFAULT); - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[1], &_report_gps_pos); - } - } + orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance, ORB_PRIO_DEFAULT); } /** diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 359dbec7b5..1c07400fd8 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1272,7 +1272,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int local_pos_sp_sub; int global_pos_sub; int triplet_sub; - int gps_pos_sub; + int gps_pos_sub[2]; int sat_info_sub; int att_pos_mocap_sub; int vision_pos_sub; @@ -1304,7 +1304,8 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.cmd_sub = -1; subs.status_sub = -1; subs.vtol_status_sub = -1; - subs.gps_pos_sub = -1; + subs.gps_pos_sub[0] = -1; + subs.gps_pos_sub[1] = -1; subs.sensor_sub = -1; subs.att_sub = -1; subs.att_sp_sub = -1; @@ -1369,7 +1370,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (log_on_start) { /* check GPS topic to get GPS time */ if (log_name_timestamp) { - if (!copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos)) { + if (!copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub[0], &buf_gps_pos)) { gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; } } @@ -1483,7 +1484,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- GPS POSITION - LOG MANAGEMENT --- */ - bool gps_pos_updated = copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos); + bool gps_pos_updated = copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub[0], &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; @@ -1720,7 +1721,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- GPS POSITION - UNIT #2 --- */ - if (copy_if_updated_multi(ORB_ID(vehicle_gps_position), 1, &subs.gps_pos_sub, &buf.dual_gps_pos)) { + if (copy_if_updated_multi(ORB_ID(vehicle_gps_position), 1, &subs.gps_pos_sub[1], &buf.dual_gps_pos)) { log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf.dual_gps_pos.time_utc_usec; log_msg.body.log_GPS.fix_type = buf.dual_gps_pos.fix_type;