Skip to content

Commit

Permalink
AP_HAL_Linux: fix SPIUARTDriver to work with GPS autodetection
Browse files Browse the repository at this point in the history
GPS auto-detection requires get_baud_rate to return non-zero.  The SPIUARTDriver was returning 0.
  • Loading branch information
peterbarker committed May 7, 2024
1 parent b66ecd8 commit 7ad81eb
Show file tree
Hide file tree
Showing 3 changed files with 9 additions and 1 deletion.
3 changes: 3 additions & 0 deletions libraries/AP_HAL_Linux/SPIUARTDriver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,14 +69,17 @@ void SPIUARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
* it's sage to update speed
*/
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
high_speed_set = true;
debug("Set higher SPI-frequency");
} else {
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
high_speed_set = false;
debug("Set lower SPI-frequency");
}
break;
default:
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
high_speed_set = false;
debug("Set lower SPI-frequency");
debug("%s: wrong baudrate (%u) for SPI-driven device. setting default speed", __func__, b);
break;
Expand Down
5 changes: 5 additions & 0 deletions libraries/AP_HAL_Linux/SPIUARTDriver.h
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,9 @@ class SPIUARTDriver : public UARTDriver {
SPIUARTDriver();
void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override;
void _timer_tick(void) override;
uint32_t get_baud_rate() const override {
return high_speed_set ? 4000000U : 1000000U;
}

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

bool _external;

bool high_speed_set;
};

}
2 changes: 1 addition & 1 deletion libraries/AP_HAL_Linux/UARTDriver.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ class UARTDriver : public AP_HAL::UARTDriver {

uint32_t bw_in_bytes_per_second() const override;

uint32_t get_baud_rate() const override { return _baudrate; }
virtual uint32_t get_baud_rate() const override { return _baudrate; }

private:
AP_HAL::OwnPtr<SerialDevice> _device;
Expand Down

0 comments on commit 7ad81eb

Please sign in to comment.