fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 05:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

applying dual gps patch

Browse Source 
resolve transfer errors

reformatting

implementing multi-topics
This commit is contained in:
Andreas Bircher
2016-05-22 12:19:47 +02:00
committed by Lorenz Meier
parent 5be1f3a856
commit 556851a511
3 changed files with 151 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/gps.cpp
+125 -48
View File
@@ -100,7 +100,7 @@ public:
class GPS
{
public:
GPS(const char *uart_path, bool fake_gps, bool enable_sat_info);
GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num);
virtual ~GPS();
virtual int init();
@@ -124,13 +124,15 @@ private:
GPSHelper *_helper; ///< instance of GPS parser
GPS_Sat_Info *_sat_info; ///< instance of GPS sat info data object
struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position
orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position
orb_advert_t _report_gps_pos_pub[2]; ///< uORB pub for gps position
int _gps_orb_instance[2]; ///< uORB multi-topic instance
struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info
orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info
float _rate; ///< position update rate
float _rate_rtcm_injection; ///< RTCM message injection rate
unsigned _last_rate_rtcm_injection_count; ///< counter for number of RTCM messages
bool _fake_gps; ///< fake gps output
int _gps_num; ///< number of GPS connected
static const int _orb_inject_data_fd_count = 4;
int _orb_inject_data_fd[_orb_inject_data_fd_count];
@@ -161,6 +163,11 @@ private:
*/
void cmd_reset();
/**
* Publish the gps struct
*/
void publish();
/**
* This is an abstraction for the poll on serial used.
*
@@ -211,20 +218,23 @@ GPS *g_dev = nullptr;
}
GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) :
GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num) :
_task_should_exit(false),
_healthy(false),
_mode_changed(false),
_mode(GPS_DRIVER_MODE_UBX),
_helper(nullptr),
_sat_info(nullptr),
_report_gps_pos_pub(nullptr),
_report_gps_pos_pub{ nullptr, nullptr},
_gps_orb_instance{ -1, -1},
_p_report_sat_info(nullptr),
_report_sat_info_pub(nullptr),
_rate(0.0f),
_rate_rtcm_injection(0.0f),
_last_rate_rtcm_injection_count(0),
_fake_gps(fake_gps)
_fake_gps(fake_gps),
_gps_num(gps_num)
{
/* store port name */
strncpy(_port, uart_path, sizeof(_port));
@@ -566,12 +576,7 @@ GPS::task_main()
/* no time and satellite information simulated */
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);
}
publish();
usleep(2e5);
@@ -628,12 +633,7 @@ GPS::task_main()
_report_gps_pos.epv = 10000.0f;
_report_gps_pos.fix_type = 0;
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);
}
publish();
/* GPS is obviously detected successfully, reset statistics */
_helper->resetUpdateRates();
@@ -644,23 +644,13 @@ GPS::task_main()
while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
if (helper_ret & 1) {
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);
}
publish();
last_rate_count++;
}
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);
}
publish();
}
/* measure update rate every 5 seconds */
@@ -810,6 +800,29 @@ GPS::print_info()
usleep(100000);
}
void
GPS::publish()
{
if (_gps_num == 1) {
if (_report_gps_pos_pub[0] != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[0], &_report_gps_pos);
} else {
_report_gps_pos_pub[0] = orb_advertise_multi(ORB_ID(vehicle_gps_position), &_report_gps_pos, &_gps_orb_instance[0], ORB_PRIO_DEFAULT);
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[0], &_report_gps_pos);
}
} else {
if (_report_gps_pos_pub[1] != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[1], &_report_gps_pos);
} else {
_report_gps_pos_pub[1] = orb_advertise_multi(ORB_ID(vehicle_gps_position), &_report_gps_pos, &_gps_orb_instance[1], ORB_PRIO_DEFAULT);
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[1], &_report_gps_pos);
}
}
}
/**
* Local functions in support of the shell command.
*/
@@ -817,8 +830,9 @@ namespace gps
{
GPS *g_dev = nullptr;
GPS *g_dev2 = 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, int gps_num);
void stop();
void test();
void reset();
@@ -828,34 +842,66 @@ void info();
* Start the driver.
*/
void
start(const char *uart_path, bool fake_gps, bool enable_sat_info)
start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num)
{
if (g_dev != nullptr) {
PX4_WARN("gps already started");
if (gps_num == 1) {
if (g_dev != nullptr) {
PX4_WARN("GPS 1 already started");
}
/* create the driver */
g_dev = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);
if (g_dev == nullptr) {
goto fail1;
}
if (OK != g_dev->init()) {
goto fail1;
}
return;
} else {
if (gps_num == 2) {
if (g_dev2 != nullptr) {
PX4_WARN("GPS 2 already started");
}
/* create the driver */
g_dev2 = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);
if (g_dev2 == nullptr) {
goto fail2;
}
if (OK != g_dev2->init()) {
goto fail2;
}
return;
}
}
/* create the driver */
g_dev = new GPS(uart_path, fake_gps, enable_sat_info);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
return;
fail:
fail1:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
PX4_ERR("start failed");
PX4_ERR("start of GPS 1 failed");
return;
fail2:
if (g_dev2 != nullptr) {
delete g_dev2;
g_dev2 = nullptr;
}
PX4_ERR("start of GPS 2 failed");
return;
}
@@ -867,6 +913,13 @@ stop()
{
delete g_dev;
g_dev = nullptr;
if (g_dev2 != nullptr) {
delete g_dev2;
}
g_dev2 = nullptr;
px4_task_exit(0);
}
/**
@@ -903,6 +956,12 @@ info()
}
g_dev->print_info();
if (g_dev2 != nullptr) {
g_dev2->print_info();
}
return;
}
} // namespace
@@ -913,6 +972,7 @@ gps_main(int argc, char *argv[])
{
/* set to default */
const char *device_name = GPS_DEFAULT_UART_PORT;
const char *device_name2 = nullptr;
bool fake_gps = false;
bool enable_sat_info = false;
@@ -949,7 +1009,24 @@ gps_main(int argc, char *argv[])
}
}
gps::start(device_name, fake_gps, enable_sat_info);
/* Allow to use a second gps device */
for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-dualgps")) {
if (argc > i + 1) {
device_name2 = argv[i + 1];
} else {
PX4_ERR("Did not get second device address");
}
}
}
gps::start(device_name, fake_gps, enable_sat_info, 1);
if (!(device_name2 == nullptr)) {
gps::start(device_name2, fake_gps, enable_sat_info, 2);
}
}
if (!strcmp(argv[1], "stop")) {