void GPS::print_info() { switch (_mode) { case GPS_DRIVER_MODE_UBX: warnx("protocol: UBX"); break; case GPS_DRIVER_MODE_MTK: warnx("protocol: MTK"); break; default: break; } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled"); if (_report_gps_pos.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); } usleep(100000); }
void GPS::print_info() { switch (_mode) { case GPS_DRIVER_MODE_UBX: warnx("protocol: UBX"); break; case GPS_DRIVER_MODE_MTK: warnx("protocol: MTK"); break; default: break; } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); if (_report.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type, _report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); } usleep(100000); }
void GPS::task_main() { log("starting"); /* open the serial port */ _serial_fd = ::open(_port, O_RDWR); if (_serial_fd < 0) { log("failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; _exit(1); } uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_fake_gps) { _report_gps_pos.timestamp_position = hrt_absolute_time(); _report_gps_pos.lat = (int32_t)18.520510e7f; _report_gps_pos.lon = (int32_t)73.856733; _report_gps_pos.alt = (int32_t)1200e3f; _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.s_variance_m_s = 10.0f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.9f; _report_gps_pos.epv = 1.8f; _report_gps_pos.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); _report_gps_pos.cog_rad = 0.0f; _report_gps_pos.vel_ned_valid = true; //no time and satellite information simulated if (!(_pub_blocked)) { if (_report_gps_pos_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } else { _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } usleep(2e5); } else { if (_Helper != nullptr) { delete(_Helper); /* set to zero to ensure parser is not used while not instantiated */ _Helper = nullptr; } switch (_mode) { case GPS_DRIVER_MODE_UBX: _Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: _Helper = new MTK(_serial_fd, &_report_gps_pos); break; default: break; } unlock(); if (_Helper->configure(_baudrate) == 0) { unlock(); // GPS is obviously detected successfully, reset statistics _Helper->reset_update_rates(); int helper_ret; while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { if (helper_ret & 1) { if (_report_gps_pos_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } else { _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } if (_p_report_sat_info && (helper_ret & 2)) { if (_report_sat_info_pub > 0) { orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); } else { _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); } } } if (helper_ret & 1) { // consider only pos info updates for rate calculation */ last_rate_count++; } /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; _Helper->store_update_rates(); _Helper->reset_update_rates(); } if (!_healthy) { const char *mode_str = "unknown"; switch (_mode) { case GPS_DRIVER_MODE_UBX: mode_str = "UBX"; break; case GPS_DRIVER_MODE_MTK: mode_str = "MTK"; break; default: break; } warnx("module found: %s", mode_str); _healthy = true; } } if (_healthy) { warnx("module lost"); _healthy = false; _rate = 0.0f; } lock(); } lock(); /* select next mode */ switch (_mode) { case GPS_DRIVER_MODE_UBX: _mode = GPS_DRIVER_MODE_MTK; break; case GPS_DRIVER_MODE_MTK: _mode = GPS_DRIVER_MODE_UBX; break; default: break; } } } warnx("exiting"); ::close(_serial_fd); /* tell the dtor that we are exiting */ _task = -1; _exit(0); }
void GPS::task_main() { /* open the serial port */ _serial_fd = ::open(_port, O_RDWR); if (_serial_fd < 0) { DEVICE_LOG("failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; _exit(1); } uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_fake_gps) { _report_gps_pos.timestamp_position = hrt_absolute_time(); _report_gps_pos.lat = (int32_t)47.378301e7f; _report_gps_pos.lon = (int32_t)8.538777e7f; _report_gps_pos.alt = (int32_t)1200e3f; _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.s_variance_m_s = 10.0f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.9f; _report_gps_pos.epv = 1.8f; _report_gps_pos.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); _report_gps_pos.cog_rad = 0.0f; _report_gps_pos.vel_ned_valid = true; /* no time and satellite information simulated */ if (!(_pub_blocked)) { if (_report_gps_pos_pub != nullptr) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } else { _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } usleep(2e5); } else { if (_Helper != nullptr) { delete(_Helper); /* set to zero to ensure parser is not used while not instantiated */ _Helper = nullptr; } switch (_mode) { case GPS_DRIVER_MODE_UBX: _Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: _Helper = new MTK(_serial_fd, &_report_gps_pos); break; case GPS_DRIVER_MODE_ASHTECH: _Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info); break; default: break; } unlock(); /* the Ashtech driver lies about successful configuration and the * MTK driver is not well tested, so we really only trust the UBX * driver for an advance publication */ if (_Helper->configure(_baudrate) == 0) { unlock(); /* reset report */ memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); if (_mode == GPS_DRIVER_MODE_UBX) { /* Publish initial report that we have access to a GPS, * but set all critical state fields to indicate we have * no valid position lock */ _report_gps_pos.timestamp_time = hrt_absolute_time(); /* reset the timestamps for data, because we have no data yet */ _report_gps_pos.timestamp_position = 0; _report_gps_pos.timestamp_variance = 0; _report_gps_pos.timestamp_velocity = 0; /* set a massive variance */ _report_gps_pos.eph = 10000.0f; _report_gps_pos.epv = 10000.0f; _report_gps_pos.fix_type = 0; if (!(_pub_blocked)) { if (_report_gps_pos_pub != nullptr) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } else { _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } /* GPS is obviously detected successfully, reset statistics */ _Helper->reset_update_rates(); } int helper_ret; while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { if (helper_ret & 1) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } if (_p_report_sat_info && (helper_ret & 2)) { if (_report_sat_info_pub != nullptr) { orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); } else { _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); } } } if (helper_ret & 1) { // consider only pos info updates for rate calculation */ last_rate_count++; } /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; _Helper->store_update_rates(); _Helper->reset_update_rates(); } if (!_healthy) { const char *mode_str = "unknown"; switch (_mode) { case GPS_DRIVER_MODE_UBX: mode_str = "UBX"; break; case GPS_DRIVER_MODE_MTK: mode_str = "MTK"; break; case GPS_DRIVER_MODE_ASHTECH: mode_str = "ASHTECH"; break; default: break; } warnx("module found: %s", mode_str); _healthy = true; } } if (_healthy) { warnx("module lost"); _healthy = false; _rate = 0.0f; } lock(); } lock(); /* select next mode */ switch (_mode) { case GPS_DRIVER_MODE_UBX: _mode = GPS_DRIVER_MODE_MTK; break; case GPS_DRIVER_MODE_MTK: _mode = GPS_DRIVER_MODE_ASHTECH; break; case GPS_DRIVER_MODE_ASHTECH: _mode = GPS_DRIVER_MODE_UBX; break; default: break; } } } warnx("exiting"); ::close(_serial_fd); /* tell the dtor that we are exiting */ _task = -1; _exit(0); }
void GPS::task_main() { log("starting"); /* open the serial port */ _serial_fd = ::open(_port, O_RDWR); if (_serial_fd < 0) { log("failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; _exit(1); } uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_Helper != nullptr) { delete(_Helper); /* set to zero to ensure parser is not used while not instantiated */ _Helper = nullptr; } switch (_mode) { case GPS_DRIVER_MODE_UBX: _Helper = new UBX(_serial_fd, &_report); break; case GPS_DRIVER_MODE_MTK: _Helper = new MTK(_serial_fd, &_report); break; case GPS_DRIVER_MODE_NMEA: //_Helper = new NMEA(); //TODO: add NMEA break; default: break; } unlock(); if (_Helper->configure(_baudrate) == 0) { unlock(); // GPS is obviously detected successfully, reset statistics _Helper->reset_update_rates(); while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); } else { _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } last_rate_count++; /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; _Helper->store_update_rates(); _Helper->reset_update_rates(); } if (!_healthy) { char *mode_str = "unknown"; switch (_mode) { case GPS_DRIVER_MODE_UBX: mode_str = "UBX"; break; case GPS_DRIVER_MODE_MTK: mode_str = "MTK"; break; case GPS_DRIVER_MODE_NMEA: mode_str = "NMEA"; break; default: break; } warnx("module found: %s", mode_str); _healthy = true; } } if (_healthy) { warnx("module lost"); _healthy = false; _rate = 0.0f; } lock(); } lock(); /* select next mode */ switch (_mode) { case GPS_DRIVER_MODE_UBX: _mode = GPS_DRIVER_MODE_MTK; break; case GPS_DRIVER_MODE_MTK: _mode = GPS_DRIVER_MODE_UBX; break; // case GPS_DRIVER_MODE_NMEA: // _mode = GPS_DRIVER_MODE_UBX; // break; default: break; } } warnx("exiting"); ::close(_serial_fd); /* tell the dtor that we are exiting */ _task = -1; _exit(0); }