fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 17:33: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")) {
src/modules/sdlog2/sdlog2.c
+24 -2
View File
@@ -1188,6 +1188,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct ekf2_replay_s replay;
struct vehicle_land_detected_s land_detected;
struct cpuload_s cpuload;
struct vehicle_gps_position_s dual_gps_pos;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -1368,7 +1369,7 @@ int sdlog2_thread_main(int argc, char *argv[])
if (log_on_start) {
/* check GPS topic to get GPS time */
if (log_name_timestamp) {
if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
if (!copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos)) {
gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
}
}
@@ -1482,7 +1483,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- GPS POSITION - LOG MANAGEMENT --- */
bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos);
bool gps_pos_updated = copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos);
if (gps_pos_updated && log_name_timestamp) {
gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
@@ -1718,6 +1719,27 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GPS);
}
/* --- GPS POSITION - UNIT #2 --- */
if (copy_if_updated_multi(ORB_ID(vehicle_gps_position), 1, &subs.gps_pos_sub, &buf.dual_gps_pos)) {
log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf.dual_gps_pos.time_utc_usec;
log_msg.body.log_GPS.fix_type = buf.dual_gps_pos.fix_type;
log_msg.body.log_GPS.eph = buf.dual_gps_pos.eph;
log_msg.body.log_GPS.epv = buf.dual_gps_pos.epv;
log_msg.body.log_GPS.lat = buf.dual_gps_pos.lat;
log_msg.body.log_GPS.lon = buf.dual_gps_pos.lon;
log_msg.body.log_GPS.alt = buf.dual_gps_pos.alt * 0.001f;
log_msg.body.log_GPS.vel_n = buf.dual_gps_pos.vel_n_m_s;
log_msg.body.log_GPS.vel_e = buf.dual_gps_pos.vel_e_m_s;
log_msg.body.log_GPS.vel_d = buf.dual_gps_pos.vel_d_m_s;
log_msg.body.log_GPS.cog = buf.dual_gps_pos.cog_rad;
log_msg.body.log_GPS.sats = buf.dual_gps_pos.satellites_used;
log_msg.body.log_GPS.snr_mean = snr_mean;
log_msg.body.log_GPS.noise_per_ms = buf.dual_gps_pos.noise_per_ms;
log_msg.body.log_GPS.jamming_indicator = buf.dual_gps_pos.jamming_indicator;
LOGBUFFER_WRITE_AND_COUNT(GPS);
}
/* --- SATELLITE INFO - UNIT #1 --- */
if (_extended_logging) {
src/modules/sdlog2/sdlog2_messages.h
+2
View File
@@ -146,6 +146,7 @@ struct log_LPSP_s {
/* --- GPS - GPS POSITION --- */
#define LOG_GPS_MSG 8
#define LOG_DGPS_MSG 58
struct log_GPS_s {
uint64_t gps_time;
uint8_t fix_type;
@@ -647,6 +648,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"),
LOG_FORMAT(LPSP, "ffffffffff", "X,Y,Z,Yaw,VX,VY,VZ,AX,AY,AZ"),
LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
LOG_FORMAT_S(DGPS, GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBB", "MainState,NavState,ArmS,Failsafe"),