fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 09:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

gpssim: cleaned up gpssim code

Browse Source 
The gpssim code was named gps_sim vs being consistent with other
simulators (gpssim). It also used warnx and errx and had lots of
commeted out code.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2015-06-18 09:02:30 +02:00
committed by Lorenz Meier
parent f56990a9ec
commit 7734195242
5 changed files with 107 additions and 200 deletions
Download Patch File Download Diff File Expand all files Collapse all files
posix-configs/SITL/init/rcS
+1 -1
View File
@@ -12,7 +12,7 @@ gyrosim start
accelsim start accelsim start
barosim start barosim start
adcsim start adcsim start
gps_sim start gpssim start
commander start commander start
sensors start sensors start
ekf_att_pos_estimator start ekf_att_pos_estimator start
src/platforms/posix/drivers/gpssim/gpssim.cpp
+102 -187
View File
@@ -32,12 +32,10 @@
****************************************************************************/ ****************************************************************************/
/** /**
* @file gps.cpp * @file gpssim.cpp
* Driver for the GPS on a serial port * Simulated GPS driver
*/ */
//#include <nuttx/clock.h>
#include <sys/types.h> #include <sys/types.h>
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
@@ -53,15 +51,9 @@
#include <unistd.h> #include <unistd.h>
#include <fcntl.h> #include <fcntl.h>
#include <px4_config.h> #include <px4_config.h>
//#include <nuttx/arch.h>
//#include <arch/board/board.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/device/device.h> #include <drivers/device/device.h>
//#include <drivers/device/i2c.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
//#include <systemlib/perf_counter.h>
//#include <systemlib/scheduling_priorities.h>
#include <systemlib/err.h>
#include <drivers/drv_gps.h> #include <drivers/drv_gps.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
@@ -69,15 +61,8 @@
#include <simulator/simulator.h> #include <simulator/simulator.h>
//#include <board_config.h>
#define GPS_DRIVER_MODE_UBX_SIM #define GPS_DRIVER_MODE_UBX_SIM
#define GPS_SIM_DEVICE_PATH "/dev/gps_sim" #define GPSSIM_DEVICE_PATH "/dev/gpssim"
//#include "ubx.h"
//#include "mtk.h"
//#include "ashtech.h"
#define TIMEOUT_5HZ 500 #define TIMEOUT_5HZ 500
#define RATE_MEASUREMENT_PERIOD 5000000 #define RATE_MEASUREMENT_PERIOD 5000000
@@ -96,11 +81,11 @@ public:
}; };
class GPS_SIM : public device::VDev class GPSSIM : public device::VDev
{ {
public: public:
GPS_SIM(const char *uart_path, bool fake_gps, bool enable_sat_info); GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info);
virtual ~GPS_SIM(); virtual ~GPSSIM();
virtual int init(); virtual int init();
@@ -157,25 +142,24 @@ private:
void cmd_reset(); void cmd_reset();
int receive(int timeout); int receive(int timeout);
}; };
/* /*
* Driver 'main' command. * Driver 'main' command.
*/ */
extern "C" __EXPORT int gps_sim_main(int argc, char *argv[]); extern "C" __EXPORT int gpssim_main(int argc, char *argv[]);
namespace namespace
{ {
GPS_SIM *g_dev = nullptr; GPSSIM *g_dev = nullptr;
} }
GPS_SIM::GPS_SIM(const char *uart_path, bool fake_gps, bool enable_sat_info) : GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info) :
VDev("gps", GPS_SIM_DEVICE_PATH), VDev("gps", GPSSIM_DEVICE_PATH),
_task_should_exit(false), _task_should_exit(false),
//_healthy(false), //_healthy(false),
//_mode_changed(false), //_mode_changed(false),
@@ -207,7 +191,7 @@ GPS_SIM::GPS_SIM(const char *uart_path, bool fake_gps, bool enable_sat_info) :
_debug_enabled = true; _debug_enabled = true;
} }
GPS_SIM::~GPS_SIM() GPSSIM::~GPSSIM()
{ {
/* tell the task we want it to go away */ /* tell the task we want it to go away */
_task_should_exit = true; _task_should_exit = true;
@@ -223,11 +207,10 @@ GPS_SIM::~GPS_SIM()
px4_task_delete(_task); px4_task_delete(_task);
g_dev = nullptr; g_dev = nullptr;
} }
int int
GPS_SIM::init() GPSSIM::init()
{ {
int ret = ERROR; int ret = ERROR;
@@ -237,10 +220,10 @@ GPS_SIM::init()
/* start the GPS driver worker task */ /* start the GPS driver worker task */
_task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPS_SIM::task_main_trampoline, nullptr); SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPSSIM::task_main_trampoline, nullptr);
if (_task < 0) { if (_task < 0) {
warnx("task start failed: %d", errno); PX4_ERR("task start failed: %d", errno);
return -errno; return -errno;
} }
@@ -250,7 +233,7 @@ out:
} }
int int
GPS_SIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) GPSSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{ {
lock(); lock();
@@ -273,13 +256,13 @@ GPS_SIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
} }
void void
GPS_SIM::task_main_trampoline(void *arg) GPSSIM::task_main_trampoline(void *arg)
{ {
g_dev->task_main(); g_dev->task_main();
} }
int int
GPS_SIM::receive(int timeout) { GPSSIM::receive(int timeout) {
Simulator *sim = Simulator::getInstance(); Simulator *sim = Simulator::getInstance();
simulator::RawGPSData gps; simulator::RawGPSData gps;
sim->getGPSSample((uint8_t *)&gps, sizeof(gps)); sim->getGPSSample((uint8_t *)&gps, sizeof(gps));
@@ -304,7 +287,7 @@ GPS_SIM::receive(int timeout) {
} }
void void
GPS_SIM::task_main() GPSSIM::task_main()
{ {
/* loop handling received serial bytes and also configuring in between */ /* loop handling received serial bytes and also configuring in between */
@@ -331,7 +314,6 @@ GPS_SIM::task_main()
//no time and satellite information simulated //no time and satellite information simulated
if (!(_pub_blocked)) { if (!(_pub_blocked)) {
if (_report_gps_pos_pub != nullptr) { if (_report_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
@@ -342,125 +324,47 @@ GPS_SIM::task_main()
} }
usleep(2e5); usleep(2e5);
} else { } else {
//Publish initial report that we have access to a GPS
//Make sure to clear any stale data in case driver is reset
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
_report_gps_pos.timestamp_position = hrt_absolute_time();
_report_gps_pos.timestamp_variance = hrt_absolute_time();
_report_gps_pos.timestamp_velocity = hrt_absolute_time();
_report_gps_pos.timestamp_time = hrt_absolute_time();
// if (_Helper != nullptr) { if (!(_pub_blocked)) {
// delete(_Helper); if (_report_gps_pos_pub != nullptr) {
// set to zero to ensure parser is not used while not instantiated orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
// _Helper = nullptr;
// }
// switch (_mode) { } else {
// case GPS_DRIVER_MODE_UBX_SIM: _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
// _Helper = new UBX_SIM(_serial_fd, &_report_gps_pos, _p_report_sat_info); }
// break; }
// default: // GPS is obviously detected successfully, reset statistics
// break; //_Helper->reset_update_rates();
// }
//if (_Helper->configure(_baudrate) == 0) { while ((receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
/* opportunistic publishing - else invalid data would end up on the bus */
//Publish initial report that we have access to a GPS
//Make sure to clear any stale data in case driver is reset
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
_report_gps_pos.timestamp_position = hrt_absolute_time();
_report_gps_pos.timestamp_variance = hrt_absolute_time();
_report_gps_pos.timestamp_velocity = hrt_absolute_time();
_report_gps_pos.timestamp_time = hrt_absolute_time();
if (!(_pub_blocked)) { if (!(_pub_blocked)) {
if (_report_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
if (_p_report_sat_info) {
if (_report_sat_info_pub != nullptr) {
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
} else { } else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
}
}
// GPS is obviously detected successfully, reset statistics
//_Helper->reset_update_rates();
while ((receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
if (!(_pub_blocked)) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
if (_p_report_sat_info) {
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_SIM:
// mode_str = "UBX";
// break;
// default:
// break;
// }
// warnx("module found: %s", mode_str);
// _healthy = true;
// }
} }
}
// if (_healthy) { lock();
// warnx("module lost");
// _healthy = false;
// _rate = 0.0f;
// }
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"); PX4_INFO("exiting");
//::close(_serial_fd);
/* tell the dtor that we are exiting */ /* tell the dtor that we are exiting */
_task = -1; _task = -1;
@@ -470,39 +374,38 @@ GPS_SIM::task_main()
void void
GPS_SIM::cmd_reset() GPSSIM::cmd_reset()
{ {
} }
void void
GPS_SIM::print_info() GPSSIM::print_info()
{ {
//GPS Mode //GPS Mode
if(_fake_gps) { if(_fake_gps) {
warnx("protocol: faked"); PX4_INFO("protocol: faked");
} }
else { else {
warnx("protocol: SIM"); PX4_INFO("protocol: SIM");
} }
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); PX4_INFO("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
warnx("sat info: %s, noise: %d, jamming detected: %s", PX4_INFO("sat info: %s, noise: %d, jamming detected: %s",
(_p_report_sat_info != nullptr) ? "enabled" : "disabled", (_p_report_sat_info != nullptr) ? "enabled" : "disabled",
_report_gps_pos.noise_per_ms, _report_gps_pos.noise_per_ms,
_report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO");
if (_report_gps_pos.timestamp_position != 0) { if (_report_gps_pos.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, PX4_INFO("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); _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); PX4_INFO("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, PX4_INFO("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); (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); PX4_INFO("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()); //PX4_INFO("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()); //PX4_INFO("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
warnx("rate publication:\t%6.2f Hz", (double)_rate); PX4_INFO("rate publication:\t%6.2f Hz", (double)_rate);
} }
@@ -512,10 +415,10 @@ GPS_SIM::print_info()
/** /**
* Local functions in support of the shell command. * Local functions in support of the shell command.
*/ */
namespace gps_sim namespace gpssim
{ {
GPS_SIM *g_dev = nullptr; GPSSIM *g_dev = nullptr;
void start(const char *uart_path, bool fake_gps, bool enable_sat_info); void start(const char *uart_path, bool fake_gps, bool enable_sat_info);
void stop(); void stop();
@@ -531,23 +434,22 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info)
{ {
int fd; int fd;
if (g_dev != nullptr)
warnx("already started");
/* create the driver */ /* create the driver */
g_dev = new GPS_SIM(uart_path, fake_gps, enable_sat_info); g_dev = new GPSSIM(uart_path, fake_gps, enable_sat_info);
if (g_dev == nullptr) if (g_dev == nullptr) {
goto fail; goto fail;
}
if (OK != g_dev->init()) if (OK != g_dev->init()) {
goto fail; goto fail;
}
/* set the poll rate to default, starts automatic data collection */ /* set the poll rate to default, starts automatic data collection */
fd = px4_open(GPS_SIM_DEVICE_PATH, O_RDONLY); fd = px4_open(GPSSIM_DEVICE_PATH, O_RDONLY);
if (fd < 0) { if (fd < 0) {
warnx("open: %s\n", GPS0_DEVICE_PATH); PX4_ERR("open: %s\n", GPS0_DEVICE_PATH);
goto fail; goto fail;
} }
@@ -560,7 +462,7 @@ fail:
g_dev = nullptr; g_dev = nullptr;
} }
warnx("start failed"); PX4_ERR("start failed");
} }
/** /**
@@ -581,8 +483,7 @@ stop()
void void
test() test()
{ {
PX4_INFO("PASS");
warnx("PASS");
} }
/** /**
@@ -591,13 +492,15 @@ test()
void void
reset() reset()
{ {
int fd = px4_open(GPS_SIM_DEVICE_PATH, O_RDONLY); int fd = px4_open(GPSSIM_DEVICE_PATH, O_RDONLY);
if (fd < 0) if (fd < 0) {
warnx("failed "); PX4_ERR("failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
warnx("reset failed"); PX4_ERR("reset failed");
}
} }
/** /**
@@ -606,8 +509,9 @@ reset()
void void
info() info()
{ {
if (g_dev == nullptr) if (g_dev == nullptr) {
errx(1, "not running"); PX4_ERR("gpssim not running");
}
g_dev->print_info(); g_dev->print_info();
} }
@@ -616,7 +520,7 @@ info()
int int
gps_sim_main(int argc, char *argv[]) gpssim_main(int argc, char *argv[])
{ {
/* set to default */ /* set to default */
@@ -628,6 +532,11 @@ gps_sim_main(int argc, char *argv[])
* Start/load the driver. * Start/load the driver.
*/ */
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr) {
PX4_WARN("gpssim already started");
return 0;
}
/* work around getopt unreliability */ /* work around getopt unreliability */
if (argc > 3) { if (argc > 3) {
if (!strcmp(argv[2], "-d")) { if (!strcmp(argv[2], "-d")) {
@@ -640,43 +549,49 @@ gps_sim_main(int argc, char *argv[])
/* Detect fake gps option */ /* Detect fake gps option */
for (int i = 2; i < argc; i++) { for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-f")) if (!strcmp(argv[i], "-f")) {
fake_gps = true; fake_gps = true;
}
} }
/* Detect sat info option */ /* Detect sat info option */
for (int i = 2; i < argc; i++) { for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-s")) if (!strcmp(argv[i], "-s")) {
enable_sat_info = true; enable_sat_info = true;
}
} }
gps_sim::start(device_name, fake_gps, enable_sat_info); gpssim::start(device_name, fake_gps, enable_sat_info);
} }
if (!strcmp(argv[1], "stop")) if (!strcmp(argv[1], "stop")) {
gps_sim::stop(); gpssim::stop();
}
/* /*
* Test the driver/device. * Test the driver/device.
*/ */
if (!strcmp(argv[1], "test")) if (!strcmp(argv[1], "test")) {
gps_sim::test(); gpssim::test();
}
/* /*
* Reset the driver. * Reset the driver.
*/ */
if (!strcmp(argv[1], "reset")) if (!strcmp(argv[1], "reset")) {
gps_sim::reset(); gpssim::reset();
}
/* /*
* Print driver status. * Print driver status.
*/ */
if (!strcmp(argv[1], "status")) if (!strcmp(argv[1], "status")) {
gps_sim::info(); gpssim::info();
}
return 0; return 0;
out: out:
warnx("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); PX4_INFO("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]");
return 1; return 1;
} }
src/platforms/posix/drivers/gpssim/module.mk
+1 -1
View File
@@ -35,7 +35,7 @@
# Simulated GPS driver # Simulated GPS driver
# #
MODULE_COMMAND = gps_sim MODULE_COMMAND = gpssim
SRCS = gpssim.cpp SRCS = gpssim.cpp
src/platforms/posix/drivers/gpssim/ubx_sim.cpp
+3 -5
View File
@@ -76,14 +76,12 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate
_gps_position(gps_position), _gps_position(gps_position),
_satellite_info(satellite_info), _satellite_info(satellite_info),
{ {
} }
UBX::~UBX() UBX::~UBX()
{ {
} }
int UBX_SIM::configure(unsigned &baudrate) int UBX_SIM::configure(unsigned &baudrate)
{ {
return 0; return 0;
@@ -92,7 +90,7 @@ int UBX_SIM::configure(unsigned &baudrate)
int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled
UBX_SIM::receive(const unsigned timeout) UBX_SIM::receive(const unsigned timeout)
{ {
/* copy data from simulator here */ /* copy data from simulator here */
usleep(1000000); usleep(1000000);
return 1; return 1;
} }
src/platforms/posix/drivers/gpssim/ubx_sim.h
-6
View File
@@ -39,9 +39,6 @@
#ifndef UBX_SIM_H_ #ifndef UBX_SIM_H_
#define UBX_SIM_H_ #define UBX_SIM_H_
class UBX_SIM class UBX_SIM
{ {
public: public:
@@ -51,9 +48,6 @@ public:
int configure(unsigned &baudrate); int configure(unsigned &baudrate);
private: private:
int _fd; int _fd;
struct vehicle_gps_position_s *_gps_position; struct vehicle_gps_position_s *_gps_position;
struct satellite_info_s *_satellite_info; struct satellite_info_s *_satellite_info;