dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fixed multi subscribing and publishing 

removed debugging message

changed to orb_publish_auto
This commit is contained in:
Sebastian Verling 2016-04-26 11:11:49 +02:00 committed by Lorenz Meier
parent 556851a511
commit c1cdef7e63
2 changed files with 11 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/drivers/gps/gps.cpp
View File

@ -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);
}
/**

11
src/modules/sdlog2/sdlog2.c
View File

@ -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;