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

GPS driver: Improve GPS "fake" mode init and code style

Browse Source
This commit is contained in:
Lorenz Meier
2017-02-17 22:28:55 +01:00
parent 495e16d510
commit bbc5186e2a
1 changed files with 14 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/gps.cpp
+14 -13
View File
@@ -253,7 +253,8 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
_interface(interface), _interface(interface),
_helper(nullptr), _helper(nullptr),
_sat_info(nullptr), _sat_info(nullptr),
_report_gps_pos_pub{nullptr}, _report_gps_pos{},
_report_gps_pos_pub(nullptr),
_gps_orb_instance(-1), _gps_orb_instance(-1),
_p_report_sat_info(nullptr), _p_report_sat_info(nullptr),
_report_sat_info_pub(nullptr), _report_sat_info_pub(nullptr),
@@ -272,8 +273,6 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
/* enforce null termination */ /* enforce null termination */
_port[sizeof(_port) - 1] = '\0'; _port[sizeof(_port) - 1] = '\0';
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
/* create satellite info data object if requested */ /* create satellite info data object if requested */
if (enable_sat_info) { if (enable_sat_info) {
_sat_info = new GPS_Sat_Info(); _sat_info = new GPS_Sat_Info();
@@ -657,15 +656,17 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
void void
GPS::task_main() GPS::task_main()
{ {
/* open the serial port */ if (!_fake_gps) {
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY); /* open the serial port */
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (_serial_fd < 0) { if (_serial_fd < 0) {
PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno); PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);
/* tell the dtor that we are exiting, set error code */ /* tell the dtor that we are exiting, set error code */
_task = -1; _task = -1;
px4_task_exit(1); px4_task_exit(1);
}
} }
_orb_inject_data_fd = orb_subscribe(ORB_ID(gps_inject_data)); _orb_inject_data_fd = orb_subscribe(ORB_ID(gps_inject_data));
@@ -679,6 +680,7 @@ GPS::task_main()
while (!_task_should_exit) { while (!_task_should_exit) {
if (_fake_gps) { if (_fake_gps) {
_report_gps_pos = {};
_report_gps_pos.timestamp = hrt_absolute_time(); _report_gps_pos.timestamp = hrt_absolute_time();
_report_gps_pos.lat = (int32_t)47.378301e7f; _report_gps_pos.lat = (int32_t)47.378301e7f;
_report_gps_pos.lon = (int32_t)8.538777e7f; _report_gps_pos.lon = (int32_t)8.538777e7f;
@@ -694,8 +696,7 @@ GPS::task_main()
_report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_n_m_s = 0.0f;
_report_gps_pos.vel_e_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_d_m_s = 0.0f;
_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_m_s = 0.0f;
_report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
_report_gps_pos.cog_rad = 0.0f; _report_gps_pos.cog_rad = 0.0f;
_report_gps_pos.vel_ned_valid = true; _report_gps_pos.vel_ned_valid = true;
_report_gps_pos.satellites_used = 10; _report_gps_pos.satellites_used = 10;
@@ -705,7 +706,7 @@ GPS::task_main()
publish(); publish();
usleep(2e5); usleep(200000);
} else { } else {