Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 18:52:46 +08:00
Code Issues Actions 28 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),
_helper(nullptr),
_sat_info(nullptr),
_report_gps_pos_pub{nullptr},
_report_gps_pos{},
_report_gps_pos_pub(nullptr),
_gps_orb_instance(-1),
_p_report_sat_info(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 */
_port[sizeof(_port) - 1] = '\0';
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
/* create satellite info data object if requested */
if (enable_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
GPS::task_main()
{
/* open the serial port */
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (!_fake_gps) {
/* open the serial port */
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (_serial_fd < 0) {
PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);
if (_serial_fd < 0) {
PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);
/* tell the dtor that we are exiting, set error code */
_task = -1;
px4_task_exit(1);
/* tell the dtor that we are exiting, set error code */
_task = -1;
px4_task_exit(1);
}
}
_orb_inject_data_fd = orb_subscribe(ORB_ID(gps_inject_data));
@@ -679,6 +680,7 @@ GPS::task_main()
while (!_task_should_exit) {
if (_fake_gps) {
_report_gps_pos = {};
_report_gps_pos.timestamp = hrt_absolute_time();
_report_gps_pos.lat = (int32_t)47.378301e7f;
_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_e_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_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
_report_gps_pos.vel_m_s = 0.0f;
_report_gps_pos.cog_rad = 0.0f;
_report_gps_pos.vel_ned_valid = true;
_report_gps_pos.satellites_used = 10;
@@ -705,7 +706,7 @@ GPS::task_main()
publish();
usleep(2e5);
usleep(200000);
} else {