mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 02:03:35 +08:00
move fake GPS to standalone module
This commit is contained in:
Daniel Agar
+138
-194
@@ -103,8 +103,8 @@ public:
|
||||
Count
|
||||
};
|
||||
|
||||
GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps, bool enable_sat_info,
|
||||
Instance instance, unsigned configured_baudrate);
|
||||
GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool enable_sat_info, Instance instance,
|
||||
unsigned configured_baudrate);
|
||||
~GPS() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
@@ -175,8 +175,6 @@ private:
|
||||
unsigned _num_bytes_read{0}; ///< counter for number of read bytes from the UART (within update interval)
|
||||
unsigned _rate_reading{0}; ///< reading rate in B/s
|
||||
|
||||
const bool _fake_gps; ///< fake gps output
|
||||
|
||||
const Instance _instance;
|
||||
|
||||
uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)};
|
||||
@@ -258,12 +256,11 @@ px4::atomic<GPS *> GPS::_secondary_instance{nullptr};
|
||||
extern "C" __EXPORT int gps_main(int argc, char *argv[]);
|
||||
|
||||
|
||||
GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps,
|
||||
bool enable_sat_info, Instance instance, unsigned configured_baudrate) :
|
||||
GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool enable_sat_info,
|
||||
Instance instance, unsigned configured_baudrate) :
|
||||
_configured_baudrate(configured_baudrate),
|
||||
_mode(mode),
|
||||
_interface(interface),
|
||||
_fake_gps(fake_gps),
|
||||
_instance(instance)
|
||||
{
|
||||
/* store port name */
|
||||
@@ -637,37 +634,35 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
|
||||
void
|
||||
GPS::run()
|
||||
{
|
||||
if (!_fake_gps) {
|
||||
/* open the serial port */
|
||||
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
|
||||
/* 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);
|
||||
return;
|
||||
}
|
||||
if (_serial_fd < 0) {
|
||||
PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);
|
||||
return;
|
||||
}
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
|
||||
if (_interface == GPSHelper::Interface::SPI) {
|
||||
int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
|
||||
int status_value = ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
|
||||
if (_interface == GPSHelper::Interface::SPI) {
|
||||
int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
|
||||
int status_value = ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
|
||||
|
||||
if (status_value < 0) {
|
||||
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
|
||||
return;
|
||||
}
|
||||
|
||||
status_value = ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
|
||||
|
||||
if (status_value < 0) {
|
||||
PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
|
||||
return;
|
||||
}
|
||||
if (status_value < 0) {
|
||||
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
|
||||
return;
|
||||
}
|
||||
|
||||
#endif /* __PX4_LINUX */
|
||||
status_value = ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
|
||||
|
||||
if (status_value < 0) {
|
||||
PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* __PX4_LINUX */
|
||||
|
||||
param_t handle = param_find("GPS_YAW_OFFSET");
|
||||
float heading_offset = 0.f;
|
||||
|
||||
@@ -719,122 +714,89 @@ GPS::run()
|
||||
|
||||
/* loop handling received serial bytes and also configuring in between */
|
||||
while (!should_exit()) {
|
||||
if (_helper != nullptr) {
|
||||
delete (_helper);
|
||||
_helper = nullptr;
|
||||
}
|
||||
|
||||
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 = 0.0f;
|
||||
_report_gps_pos.cog_rad = 0.0f;
|
||||
_report_gps_pos.vel_ned_valid = true;
|
||||
_report_gps_pos.satellites_used = 10;
|
||||
_report_gps_pos.heading = NAN;
|
||||
_report_gps_pos.heading_offset = NAN;
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_NONE:
|
||||
_mode = GPS_DRIVER_MODE_UBX;
|
||||
|
||||
/* no time and satellite information simulated */
|
||||
|
||||
|
||||
publish();
|
||||
|
||||
px4_usleep(200000);
|
||||
|
||||
} else {
|
||||
|
||||
if (_helper != nullptr) {
|
||||
delete (_helper);
|
||||
_helper = nullptr;
|
||||
}
|
||||
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_NONE:
|
||||
_mode = GPS_DRIVER_MODE_UBX;
|
||||
|
||||
/* FALLTHROUGH */
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
|
||||
gps_ubx_dynmodel, heading_offset, ubx_mode);
|
||||
break;
|
||||
/* FALLTHROUGH */
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
|
||||
gps_ubx_dynmodel, heading_offset, ubx_mode);
|
||||
break;
|
||||
#ifndef CONSTRAINED_FLASH
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos);
|
||||
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, heading_offset);
|
||||
break;
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset);
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_EMLIDREACH:
|
||||
_helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
|
||||
break;
|
||||
case GPS_DRIVER_MODE_EMLIDREACH:
|
||||
_helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
|
||||
break;
|
||||
#endif // CONSTRAINED_FLASH
|
||||
|
||||
default:
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
_baudrate = _configured_baudrate;
|
||||
GPSHelper::GPSConfig gpsConfig{};
|
||||
gpsConfig.output_mode = GPSHelper::OutputMode::GPS;
|
||||
gpsConfig.gnss_systems = static_cast<GPSHelper::GNSSSystemsMask>(gnssSystemsParam);
|
||||
|
||||
if (_helper && _helper->configure(_baudrate, gpsConfig) == 0) {
|
||||
|
||||
/* reset report */
|
||||
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
|
||||
_report_gps_pos.heading = NAN;
|
||||
_report_gps_pos.heading_offset = heading_offset;
|
||||
|
||||
if (_mode == GPS_DRIVER_MODE_UBX) {
|
||||
|
||||
/* GPS is obviously detected successfully, reset statistics */
|
||||
_helper->resetUpdateRates();
|
||||
}
|
||||
|
||||
_baudrate = _configured_baudrate;
|
||||
GPSHelper::GPSConfig gpsConfig{};
|
||||
gpsConfig.output_mode = GPSHelper::OutputMode::GPS;
|
||||
gpsConfig.gnss_systems = static_cast<GPSHelper::GNSSSystemsMask>(gnssSystemsParam);
|
||||
int helper_ret;
|
||||
|
||||
if (_helper && _helper->configure(_baudrate, gpsConfig) == 0) {
|
||||
while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !should_exit()) {
|
||||
|
||||
/* reset report */
|
||||
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
|
||||
_report_gps_pos.heading = NAN;
|
||||
_report_gps_pos.heading_offset = heading_offset;
|
||||
if (helper_ret & 1) {
|
||||
publish();
|
||||
|
||||
if (_mode == GPS_DRIVER_MODE_UBX) {
|
||||
last_rate_count++;
|
||||
}
|
||||
|
||||
/* GPS is obviously detected successfully, reset statistics */
|
||||
if (_p_report_sat_info && (helper_ret & 2)) {
|
||||
publishSatelliteInfo();
|
||||
}
|
||||
|
||||
reset_if_scheduled();
|
||||
|
||||
/* 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;
|
||||
_rate_reading = _num_bytes_read / dt;
|
||||
last_rate_measurement = hrt_absolute_time();
|
||||
last_rate_count = 0;
|
||||
_last_rate_rtcm_injection_count = 0;
|
||||
_num_bytes_read = 0;
|
||||
_helper->storeUpdateRates();
|
||||
_helper->resetUpdateRates();
|
||||
}
|
||||
|
||||
int helper_ret;
|
||||
|
||||
while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !should_exit()) {
|
||||
|
||||
if (helper_ret & 1) {
|
||||
publish();
|
||||
|
||||
last_rate_count++;
|
||||
}
|
||||
|
||||
if (_p_report_sat_info && (helper_ret & 2)) {
|
||||
publishSatelliteInfo();
|
||||
}
|
||||
|
||||
reset_if_scheduled();
|
||||
|
||||
/* 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;
|
||||
_rate_reading = _num_bytes_read / dt;
|
||||
last_rate_measurement = hrt_absolute_time();
|
||||
last_rate_count = 0;
|
||||
_last_rate_rtcm_injection_count = 0;
|
||||
_num_bytes_read = 0;
|
||||
_helper->storeUpdateRates();
|
||||
_helper->resetUpdateRates();
|
||||
}
|
||||
|
||||
if (!_healthy) {
|
||||
// Helpful for debugging, but too verbose for normal ops
|
||||
if (!_healthy) {
|
||||
// Helpful for debugging, but too verbose for normal ops
|
||||
// const char *mode_str = "unknown";
|
||||
//
|
||||
// switch (_mode) {
|
||||
@@ -859,46 +821,44 @@ GPS::run()
|
||||
// }
|
||||
//
|
||||
// PX4_WARN("module found: %s", mode_str);
|
||||
_healthy = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (_healthy) {
|
||||
_healthy = false;
|
||||
_rate = 0.0f;
|
||||
_rate_rtcm_injection = 0.0f;
|
||||
_healthy = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (_mode_auto) {
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
if (_healthy) {
|
||||
_healthy = false;
|
||||
_rate = 0.0f;
|
||||
_rate_rtcm_injection = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
if (_mode_auto) {
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
#ifndef CONSTRAINED_FLASH
|
||||
_mode = GPS_DRIVER_MODE_MTK;
|
||||
break;
|
||||
_mode = GPS_DRIVER_MODE_MTK;
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_mode = GPS_DRIVER_MODE_ASHTECH;
|
||||
break;
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_mode = GPS_DRIVER_MODE_ASHTECH;
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
_mode = GPS_DRIVER_MODE_EMLIDREACH;
|
||||
break;
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
_mode = GPS_DRIVER_MODE_EMLIDREACH;
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_EMLIDREACH:
|
||||
case GPS_DRIVER_MODE_EMLIDREACH:
|
||||
#endif // CONSTRAINED_FLASH
|
||||
_mode = GPS_DRIVER_MODE_UBX;
|
||||
px4_usleep(500000); // tried all possible drivers. Wait a bit before next round
|
||||
break;
|
||||
_mode = GPS_DRIVER_MODE_UBX;
|
||||
px4_usleep(500000); // tried all possible drivers. Wait a bit before next round
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
} else {
|
||||
px4_usleep(500000);
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
} else {
|
||||
px4_usleep(500000);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -927,33 +887,28 @@ GPS::print_status()
|
||||
break;
|
||||
}
|
||||
|
||||
//GPS Mode
|
||||
if (_fake_gps) {
|
||||
PX4_INFO("protocol: SIMULATED");
|
||||
|
||||
} else {
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
PX4_INFO("protocol: UBX");
|
||||
break;
|
||||
// GPS Mode
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
PX4_INFO("protocol: UBX");
|
||||
break;
|
||||
#ifndef CONSTRAINED_FLASH
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
PX4_INFO("protocol: MTK");
|
||||
break;
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
PX4_INFO("protocol: MTK");
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
PX4_INFO("protocol: ASHTECH");
|
||||
break;
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
PX4_INFO("protocol: ASHTECH");
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_EMLIDREACH:
|
||||
PX4_INFO("protocol: EMLIDREACH");
|
||||
break;
|
||||
case GPS_DRIVER_MODE_EMLIDREACH:
|
||||
PX4_INFO("protocol: EMLIDREACH");
|
||||
break;
|
||||
#endif // CONSTRAINED_FLASH
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
PX4_INFO("status: %s, port: %s, baudrate: %d", _healthy ? "OK" : "NOT OK", _port, _baudrate);
|
||||
@@ -966,10 +921,8 @@ GPS::print_status()
|
||||
PX4_INFO("rate velocity: \t\t%6.2f Hz", (double)_helper->getVelocityUpdateRate());
|
||||
}
|
||||
|
||||
if (!_fake_gps) {
|
||||
PX4_INFO("rate publication:\t\t%6.2f Hz", (double)_rate);
|
||||
PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection);
|
||||
}
|
||||
PX4_INFO("rate publication:\t\t%6.2f Hz", (double)_rate);
|
||||
PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection);
|
||||
|
||||
print_message(_report_gps_pos);
|
||||
}
|
||||
@@ -1097,9 +1050,6 @@ There is a thread for each device polling for data. The GPS protocol classes are
|
||||
so that they can be used in other projects as well (eg. QGroundControl uses them too).
|
||||
|
||||
### Examples
|
||||
For testing it can be useful to fake a GPS signal (it will signal the system that it has a valid position):
|
||||
$ gps stop
|
||||
$ gps start -f
|
||||
|
||||
Starting 2 GPS devices (the main GPS on /dev/ttyS3 and the secondary on /dev/ttyS4):
|
||||
$ gps start -d /dev/ttyS3 -e /dev/ttyS4
|
||||
@@ -1115,7 +1065,6 @@ $ gps reset warm
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Optional secondary GPS device", true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('g', 0, 0, 3000000, "Baudrate (secondary GPS, can also be p:<param_name>)", true);
|
||||
|
||||
PRINT_MODULE_USAGE_PARAM_FLAG('f', "Fake a GPS signal (useful for testing)", true);
|
||||
PRINT_MODULE_USAGE_PARAM_FLAG('s', "Enable publication of satellite info", true);
|
||||
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('i', "uart", "spi|uart", "GPS interface", true);
|
||||
@@ -1186,7 +1135,6 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
|
||||
const char *device_name_secondary = nullptr;
|
||||
int baudrate_main = 0;
|
||||
int baudrate_secondary = 0;
|
||||
bool fake_gps = false;
|
||||
bool enable_sat_info = false;
|
||||
GPSHelper::Interface interface = GPSHelper::Interface::UART;
|
||||
GPSHelper::Interface interface_secondary = GPSHelper::Interface::UART;
|
||||
@@ -1197,7 +1145,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
|
||||
int ch;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "b:d:e:fg:si:j:p:", &myoptind, &myoptarg)) != EOF) {
|
||||
while ((ch = px4_getopt(argc, argv, "b:d:e:g:si:j:p:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'b':
|
||||
if (px4_get_parameter_value(myoptarg, baudrate_main) != 0) {
|
||||
@@ -1220,10 +1168,6 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
|
||||
device_name_secondary = myoptarg;
|
||||
break;
|
||||
|
||||
case 'f':
|
||||
fake_gps = true;
|
||||
break;
|
||||
|
||||
case 's':
|
||||
enable_sat_info = true;
|
||||
break;
|
||||
@@ -1290,7 +1234,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
|
||||
|
||||
GPS *gps;
|
||||
if (instance == Instance::Main) {
|
||||
gps = new GPS(device_name, mode, interface, fake_gps, enable_sat_info, instance, baudrate_main);
|
||||
gps = new GPS(device_name, mode, interface, enable_sat_info, instance, baudrate_main);
|
||||
|
||||
if (gps && device_name_secondary) {
|
||||
task_spawn(argc, argv, Instance::Secondary);
|
||||
@@ -1308,7 +1252,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
|
||||
}
|
||||
}
|
||||
} else { // secondary instance
|
||||
gps = new GPS(device_name_secondary, mode, interface_secondary, fake_gps, enable_sat_info, instance, baudrate_secondary);
|
||||
gps = new GPS(device_name_secondary, mode, interface_secondary, enable_sat_info, instance, baudrate_secondary);
|
||||
}
|
||||
|
||||
return gps;
|
||||
|
||||
Reference in New Issue
Block a user