Skip to content

Commit 7ad81eb

Browse files
committed
AP_HAL_Linux: fix SPIUARTDriver to work with GPS autodetection
GPS auto-detection requires get_baud_rate to return non-zero. The SPIUARTDriver was returning 0.
1 parent b66ecd8 commit 7ad81eb

File tree

3 files changed

+9
-1
lines changed

3 files changed

+9
-1
lines changed

libraries/AP_HAL_Linux/SPIUARTDriver.cpp

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -69,14 +69,17 @@ void SPIUARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
6969
* it's sage to update speed
7070
*/
7171
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
72+
high_speed_set = true;
7273
debug("Set higher SPI-frequency");
7374
} else {
7475
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
76+
high_speed_set = false;
7577
debug("Set lower SPI-frequency");
7678
}
7779
break;
7880
default:
7981
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
82+
high_speed_set = false;
8083
debug("Set lower SPI-frequency");
8184
debug("%s: wrong baudrate (%u) for SPI-driven device. setting default speed", __func__, b);
8285
break;

libraries/AP_HAL_Linux/SPIUARTDriver.h

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,9 @@ class SPIUARTDriver : public UARTDriver {
1111
SPIUARTDriver();
1212
void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override;
1313
void _timer_tick(void) override;
14+
uint32_t get_baud_rate() const override {
15+
return high_speed_set ? 4000000U : 1000000U;
16+
}
1417

1518
protected:
1619
int _write_fd(const uint8_t *buf, uint16_t n) override;
@@ -23,6 +26,8 @@ class SPIUARTDriver : public UARTDriver {
2326
uint32_t _last_update_timestamp;
2427

2528
bool _external;
29+
30+
bool high_speed_set;
2631
};
2732

2833
}

libraries/AP_HAL_Linux/UARTDriver.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ class UARTDriver : public AP_HAL::UARTDriver {
5656

5757
uint32_t bw_in_bytes_per_second() const override;
5858

59-
uint32_t get_baud_rate() const override { return _baudrate; }
59+
virtual uint32_t get_baud_rate() const override { return _baudrate; }
6060

6161
private:
6262
AP_HAL::OwnPtr<SerialDevice> _device;

0 commit comments

Comments
 (0)