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

Merge pull request #588 from thomasgubler/fake_gps 

Fake gps
This commit is contained in:
Lorenz Meier 2014-01-10 16:18:22 -08:00
parent 6604d7b947 bccd0f8947
commit 42268347d4
1 changed files with 132 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -85,7 +85,7 @@ static const int ERROR = -1;
class GPS : public device::CDev class GPS : public device::CDev
{ {
public: public:
GPS(const char *uart_path); GPS(const char *uart_path, bool fake_gps);
virtual ~GPS(); virtual ~GPS();
virtual int init(); virtual int init();
@ -112,6 +112,7 @@ private:
struct vehicle_gps_position_s _report; ///< uORB topic for gps position struct vehicle_gps_position_s _report; ///< uORB topic for gps position
orb_advert_t _report_pub; ///< uORB pub for gps position orb_advert_t _report_pub; ///< uORB pub for gps position
float _rate; ///< position update rate float _rate; ///< position update rate
bool _fake_gps; ///< fake gps output
/** /**
@ -156,7 +157,7 @@ GPS *g_dev;
} }
GPS::GPS(const char *uart_path) : GPS::GPS(const char *uart_path, bool fake_gps) :
CDev("gps", GPS_DEVICE_PATH), CDev("gps", GPS_DEVICE_PATH),
_task_should_exit(false), _task_should_exit(false),
_healthy(false), _healthy(false),
@ -164,7 +165,8 @@ GPS::GPS(const char *uart_path) :
_mode(GPS_DRIVER_MODE_UBX), _mode(GPS_DRIVER_MODE_UBX),
_Helper(nullptr), _Helper(nullptr),
_report_pub(-1), _report_pub(-1),
_rate(0.0f) _rate(0.0f),
_fake_gps(fake_gps)
{ {
/* store port name */ /* store port name */
strncpy(_port, uart_path, sizeof(_port)); strncpy(_port, uart_path, sizeof(_port));
@ -264,98 +266,133 @@ GPS::task_main()
/* loop handling received serial bytes and also configuring in between */ /* loop handling received serial bytes and also configuring in between */
while (!_task_should_exit) { while (!_task_should_exit) {
if (_Helper != nullptr) { if (_fake_gps) {
delete(_Helper);
/* set to zero to ensure parser is not used while not instantiated */
_Helper = nullptr;
}
switch (_mode) { _report.timestamp_position = hrt_absolute_time();
case GPS_DRIVER_MODE_UBX: _report.lat = (int32_t)47.378301e7f;
_Helper = new UBX(_serial_fd, &_report); _report.lon = (int32_t)8.538777e7f;
break; _report.alt = (int32_t)400e3f;
_report.timestamp_variance = hrt_absolute_time();
_report.s_variance_m_s = 10.0f;
_report.p_variance_m = 10.0f;
_report.c_variance_rad = 0.1f;
_report.fix_type = 3;
_report.eph_m = 10.0f;
_report.epv_m = 10.0f;
_report.timestamp_velocity = hrt_absolute_time();
_report.vel_n_m_s = 0.0f;
_report.vel_e_m_s = 0.0f;
_report.vel_d_m_s = 0.0f;
_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
_report.cog_rad = 0.0f;
_report.vel_ned_valid = true;
case GPS_DRIVER_MODE_MTK: //no time and satellite information simulated
_Helper = new MTK(_serial_fd, &_report);
break;
default: if (_report_pub > 0) {
break; orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
}
unlock(); } else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
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;
default:
break;
}
warnx("module found: %s", mode_str);
_healthy = true;
}
} }
if (_healthy) { usleep(2e5);
warnx("module lost");
_healthy = false; } else {
_rate = 0.0f;
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;
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;
default:
break;
}
warnx("module found: %s", mode_str);
_healthy = true;
}
}
if (_healthy) {
warnx("module lost");
_healthy = false;
_rate = 0.0f;
}
lock();
} }
lock(); lock();
}
lock(); /* select next mode */
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_mode = GPS_DRIVER_MODE_MTK;
break;
/* select next mode */ case GPS_DRIVER_MODE_MTK:
switch (_mode) { _mode = GPS_DRIVER_MODE_UBX;
case GPS_DRIVER_MODE_UBX: break;
_mode = GPS_DRIVER_MODE_MTK;
break;
case GPS_DRIVER_MODE_MTK: default:
_mode = GPS_DRIVER_MODE_UBX; break;
break; }
default:
break;
} }
} }
@ -417,7 +454,7 @@ namespace gps
GPS *g_dev; GPS *g_dev;
void start(const char *uart_path); void start(const char *uart_path, bool fake_gps);
void stop(); void stop();
void test(); void test();
void reset(); void reset();
@ -427,7 +464,7 @@ void info();
* Start the driver. * Start the driver.
*/ */
void void
start(const char *uart_path) start(const char *uart_path, bool fake_gps)
{ {
int fd; int fd;
@ -435,7 +472,7 @@ start(const char *uart_path)
errx(1, "already started"); errx(1, "already started");
/* create the driver */ /* create the driver */
g_dev = new GPS(uart_path); g_dev = new GPS(uart_path, fake_gps);
if (g_dev == nullptr) if (g_dev == nullptr)
goto fail; goto fail;
@ -527,6 +564,7 @@ gps_main(int argc, char *argv[])
/* set to default */ /* set to default */
char *device_name = GPS_DEFAULT_UART_PORT; char *device_name = GPS_DEFAULT_UART_PORT;
bool fake_gps = false;
/* /*
* Start/load the driver. * Start/load the driver.
@ -542,7 +580,13 @@ gps_main(int argc, char *argv[])
} }
} }
gps::start(device_name); /* Detect fake gps option */
for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-f"))
fake_gps = true;
}
gps::start(device_name, fake_gps);
} }
if (!strcmp(argv[1], "stop")) if (!strcmp(argv[1], "stop"))
@ -567,5 +611,5 @@ gps_main(int argc, char *argv[])
gps::info(); gps::info();
out: out:
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]");
} }