mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
AP_HAL_SMACCM: Fix pin conflict between uartC and I2C.
- uartC is currently disconnected. - Set uartB to USART6 for GPS communication.
This commit is contained in:
parent
7827a4a54a
commit
46b0742710
@ -8,8 +8,8 @@ using namespace SMACCM;
|
|||||||
|
|
||||||
// XXX make sure these are assigned correctly
|
// XXX make sure these are assigned correctly
|
||||||
static SMACCMUARTDriver uartADriver(usart1);
|
static SMACCMUARTDriver uartADriver(usart1);
|
||||||
static SMACCMUARTDriver uartBDriver(usart2);
|
static SMACCMUARTDriver uartBDriver(usart6);
|
||||||
static SMACCMUARTDriver uartCDriver(usart3);
|
static SMACCMUARTDriver uartCDriver(NULL);
|
||||||
|
|
||||||
static SMACCMI2CDriver i2cDriver;
|
static SMACCMI2CDriver i2cDriver;
|
||||||
static SMACCMSPIDeviceManager spiDeviceManager;
|
static SMACCMSPIDeviceManager spiDeviceManager;
|
||||||
|
@ -23,8 +23,11 @@ SMACCMUARTDriver::SMACCMUARTDriver(struct usart *dev)
|
|||||||
|
|
||||||
void SMACCMUARTDriver::begin(uint32_t baud)
|
void SMACCMUARTDriver::begin(uint32_t baud)
|
||||||
{
|
{
|
||||||
usart_init(m_dev, baud);
|
if (m_dev != NULL) {
|
||||||
usart_enable(m_dev);
|
usart_init(m_dev, baud);
|
||||||
|
usart_enable(m_dev);
|
||||||
|
}
|
||||||
|
|
||||||
m_initialized = true;
|
m_initialized = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -58,7 +61,11 @@ void SMACCMUARTDriver::set_blocking_writes(bool blocking)
|
|||||||
|
|
||||||
bool SMACCMUARTDriver::tx_pending()
|
bool SMACCMUARTDriver::tx_pending()
|
||||||
{
|
{
|
||||||
return usart_is_tx_pending(m_dev);
|
if (m_dev != NULL) {
|
||||||
|
return usart_is_tx_pending(m_dev);
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* SMACCM implementations of BetterStream virtual methods */
|
/* SMACCM implementations of BetterStream virtual methods */
|
||||||
@ -107,12 +114,18 @@ void SMACCMUARTDriver::vprintf_P(const prog_char *pstr, va_list ap)
|
|||||||
/* SMACCM implementations of Stream virtual methods */
|
/* SMACCM implementations of Stream virtual methods */
|
||||||
int16_t SMACCMUARTDriver::available()
|
int16_t SMACCMUARTDriver::available()
|
||||||
{
|
{
|
||||||
return (int16_t)usart_available(m_dev);
|
if (m_dev != NULL)
|
||||||
|
return (int16_t)usart_available(m_dev);
|
||||||
|
|
||||||
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int16_t SMACCMUARTDriver::txspace()
|
int16_t SMACCMUARTDriver::txspace()
|
||||||
{
|
{
|
||||||
return (int16_t)usart_txspace(m_dev);
|
if (m_dev != NULL)
|
||||||
|
return (int16_t)usart_txspace(m_dev);
|
||||||
|
|
||||||
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// It looks like this should always be a non-blocking read, so return
|
// It looks like this should always be a non-blocking read, so return
|
||||||
@ -121,6 +134,9 @@ int16_t SMACCMUARTDriver::read()
|
|||||||
{
|
{
|
||||||
uint8_t c;
|
uint8_t c;
|
||||||
|
|
||||||
|
if (m_dev == NULL)
|
||||||
|
return -1;
|
||||||
|
|
||||||
if (usart_read_timeout(m_dev, 0, &c, 1) == 0)
|
if (usart_read_timeout(m_dev, 0, &c, 1) == 0)
|
||||||
return -1;
|
return -1;
|
||||||
|
|
||||||
@ -131,6 +147,9 @@ int16_t SMACCMUARTDriver::peek()
|
|||||||
{
|
{
|
||||||
uint8_t c;
|
uint8_t c;
|
||||||
|
|
||||||
|
if (m_dev == NULL)
|
||||||
|
return -1;
|
||||||
|
|
||||||
if (!usart_peek(m_dev, &c))
|
if (!usart_peek(m_dev, &c))
|
||||||
return -1;
|
return -1;
|
||||||
|
|
||||||
@ -140,6 +159,9 @@ int16_t SMACCMUARTDriver::peek()
|
|||||||
/* SMACCM implementations of Print virtual methods */
|
/* SMACCM implementations of Print virtual methods */
|
||||||
size_t SMACCMUARTDriver::write(uint8_t c)
|
size_t SMACCMUARTDriver::write(uint8_t c)
|
||||||
{
|
{
|
||||||
|
if (m_dev == NULL)
|
||||||
|
return 1;
|
||||||
|
|
||||||
portTickType delay = m_blocking ? portMAX_DELAY : 0;
|
portTickType delay = m_blocking ? portMAX_DELAY : 0;
|
||||||
return usart_write_timeout(m_dev, delay, &c, 1);
|
return usart_write_timeout(m_dev, delay, &c, 1);
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user