Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 02:36:37 +08:00
Code Issues Actions 32 Packages Projects Releases Wiki Activity

drivers/gps: close port if configure fails

Browse Source 
- open if necessary at beginning of each iteration
This commit is contained in:
Daniel Agar
2021-07-04 14:15:11 -04:00
parent 3143756d97
commit 1ee3484827
1 changed files with 35 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/gps.cpp
+35 -34
View File
@@ -666,35 +666,6 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
void void
GPS::run() GPS::run()
{ {
/* 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);
return;
}
#ifdef __PX4_LINUX
if (_interface == GPSHelper::Interface::SPI) {
int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
if (status_value < 0) {
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
return;
}
status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
if (status_value < 0) {
PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
return;
}
}
#endif /* __PX4_LINUX */
param_t handle = param_find("GPS_YAW_OFFSET"); param_t handle = param_find("GPS_YAW_OFFSET");
float heading_offset = 0.f; float heading_offset = 0.f;
@@ -751,6 +722,36 @@ GPS::run()
_helper = nullptr; _helper = nullptr;
} }
if (_serial_fd < 0) {
/* open the serial port */
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (_serial_fd < 0) {
PX4_ERR("failed to open %s err: %d", _port, errno);
px4_sleep(1);
continue;
}
#ifdef __PX4_LINUX
if (_interface == GPSHelper::Interface::SPI) {
int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
if (status_value < 0) {
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
}
status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
if (status_value < 0) {
PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
}
}
#endif /* __PX4_LINUX */
}
switch (_mode) { switch (_mode) {
case GPS_DRIVER_MODE_NONE: case GPS_DRIVER_MODE_NONE:
_mode = GPS_DRIVER_MODE_UBX; _mode = GPS_DRIVER_MODE_UBX;
@@ -911,6 +912,11 @@ GPS::run()
} }
} }
if (_serial_fd >= 0) {
::close(_serial_fd);
_serial_fd = -1;
}
if (_mode_auto) { if (_mode_auto) {
switch (_mode) { switch (_mode) {
case GPS_DRIVER_MODE_UBX: case GPS_DRIVER_MODE_UBX:
@@ -946,11 +952,6 @@ GPS::run()
} }
PX4_INFO("exiting"); PX4_INFO("exiting");
if (_serial_fd >= 0) {
::close(_serial_fd);
_serial_fd = -1;
}
} }
int int