void GPS::print_info() { PX4_WARN("GPS %i:", _gps_num); //GPS Mode if (_fake_gps) { PX4_WARN("protocol: SIMULATED"); } else { switch (_mode) { case GPS_DRIVER_MODE_UBX: PX4_WARN("protocol: UBX"); break; case GPS_DRIVER_MODE_MTK: PX4_WARN("protocol: MTK"); break; case GPS_DRIVER_MODE_ASHTECH: PX4_WARN("protocol: ASHTECH"); break; default: break; } } PX4_WARN("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); PX4_WARN("sat info: %s, noise: %d, jamming detected: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled", _report_gps_pos.noise_per_ms, _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); if (_report_gps_pos.timestamp != 0) { PX4_WARN("position lock: %d, 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) / 1000.0); PX4_WARN("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); PX4_WARN("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); PX4_WARN("hdop: %.2f, vdop: %.2f", (double)_report_gps_pos.hdop, (double)_report_gps_pos.vdop); PX4_WARN("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); PX4_WARN("rate position: \t\t%6.2f Hz", (double)_helper->getPositionUpdateRate()); PX4_WARN("rate velocity: \t\t%6.2f Hz", (double)_helper->getVelocityUpdateRate()); PX4_WARN("rate publication:\t\t%6.2f Hz", (double)_rate); PX4_WARN("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection); } usleep(100000); }
void GPS::task_main() { /* open the serial port */ _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); if (_serial_fd < 0) { PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; px4_task_exit(1); } _orb_inject_data_fd = orb_subscribe(ORB_ID(gps_inject_data)); initializeCommunicationDump(); 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 = 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.alt_ellipsoid = 10000; _report_gps_pos.s_variance_m_s = 0.5f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.8f; _report_gps_pos.epv = 1.2f; _report_gps_pos.hdop = 0.9f; _report_gps_pos.vdop = 0.9f; _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; _report_gps_pos.satellites_used = 10; /* no time and satellite information simulated */ publish(); 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_NONE: _mode = GPS_DRIVER_MODE_UBX; //no break case GPS_DRIVER_MODE_UBX: _helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: _helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos); break; case GPS_DRIVER_MODE_ASHTECH: _helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); break; default: break; } /* 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, GPSHelper::OutputMode::GPS) == 0) { /* 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 */ /* reset the timestamp for data, because we have no data yet */ _report_gps_pos.timestamp = 0; _report_gps_pos.timestamp_time_relative = 0; /* set a massive variance */ _report_gps_pos.eph = 10000.0f; _report_gps_pos.epv = 10000.0f; _report_gps_pos.fix_type = 0; publish(); /* GPS is obviously detected successfully, reset statistics */ _helper->resetUpdateRates(); } int helper_ret; while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { if (helper_ret & 1) { publish(); last_rate_count++; } if (_p_report_sat_info && (helper_ret & 2)) { publishSatelliteInfo(); } /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; _rate = last_rate_count / dt; _rate_rtcm_injection = _last_rate_rtcm_injection_count / dt; last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; _last_rate_rtcm_injection_count = 0; _helper->storeUpdateRates(); _helper->resetUpdateRates(); } if (!_healthy) { // Helpful for debugging, but too verbose for normal ops // 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; // } // // PX4_WARN("module found: %s", mode_str); _healthy = true; } } if (_healthy) { PX4_WARN("GPS module lost"); _healthy = false; _rate = 0.0f; _rate_rtcm_injection = 0.0f; } } if (_mode_auto) { 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; } } } } PX4_INFO("exiting"); orb_unsubscribe(_orb_inject_data_fd); if (_dump_communication_pub) { orb_unadvertise(_dump_communication_pub); } ::close(_serial_fd); orb_unadvertise(_report_gps_pos_pub); /* tell the dtor that we are exiting */ _task = -1; px4_task_exit(0); }
void GPS::task_main() { /* open the serial port */ _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); if (_serial_fd < 0) { PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; px4_task_exit(1); } #ifndef __PX4_QURT // TODO: this call is not supported on Snapdragon just yet. // However it seems to be nonblocking anyway and working. int flags = fcntl(_serial_fd, F_GETFL, 0); 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); } initializeCommunicationDump(); 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 = 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.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.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 */ publish(); 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 GPSDriverUBX(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: _helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos); break; case GPS_DRIVER_MODE_ASHTECH: _helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); break; default: break; } /* 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, GPSHelper::OutputMode::GPS) == 0) { /* 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 */ /* reset the timestamp for data, because we have no data yet */ _report_gps_pos.timestamp = 0; _report_gps_pos.timestamp_time_relative = 0; /* set a massive variance */ _report_gps_pos.eph = 10000.0f; _report_gps_pos.epv = 10000.0f; _report_gps_pos.fix_type = 0; publish(); /* GPS is obviously detected successfully, reset statistics */ _helper->resetUpdateRates(); } int helper_ret; while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { if (helper_ret & 1) { publish(); last_rate_count++; } if (_p_report_sat_info && (helper_ret & 2)) { publishSatelliteInfo(); } /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; _rate = last_rate_count / dt; _rate_rtcm_injection = _last_rate_rtcm_injection_count / dt; last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; _last_rate_rtcm_injection_count = 0; _helper->storeUpdateRates(); _helper->resetUpdateRates(); } if (!_healthy) { // Helpful for debugging, but too verbose for normal ops // 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; // } // // PX4_WARN("module found: %s", mode_str); _healthy = true; } //check for disarming if (_vehicle_status_sub != -1 && _dump_from_gps_device_fd != -1) { bool updated; orb_check(_vehicle_status_sub, &updated); if (updated) { vehicle_status_s vehicle_status; orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status); bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); if (armed) { _is_armed = true; } else if (_is_armed) { //disable communication dump when disarming close(_dump_from_gps_device_fd); _dump_from_gps_device_fd = -1; close(_dump_to_gps_device_fd); _dump_to_gps_device_fd = -1; _is_armed = false; } } } } if (_healthy) { PX4_WARN("GPS module lost"); _healthy = false; _rate = 0.0f; _rate_rtcm_injection = 0.0f; } } /* 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; } } } PX4_WARN("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; } if (_dump_to_gps_device_fd != -1) { close(_dump_to_gps_device_fd); _dump_to_gps_device_fd = -1; } if (_dump_from_gps_device_fd != -1) { close(_dump_from_gps_device_fd); _dump_from_gps_device_fd = -1; } if (_vehicle_status_sub != -1) { orb_unsubscribe(_vehicle_status_sub); _vehicle_status_sub = -1; } ::close(_serial_fd); orb_unadvertise(_report_gps_pos_pub); /* tell the dtor that we are exiting */ _task = -1; px4_task_exit(0); }