AP_HAL_FLYMAPLE: UARTDriver libmaple internal ring buffers enlarge on demand
This commit is contained in:
parent
adfcdca074
commit
db2b7ed4e3
@ -80,11 +80,11 @@ Pins
|
||||
30 Telemetry Rx from radio on Serial3 on connector labelled "GPS". 5V tolerant.
|
||||
|
||||
Timers
|
||||
SysTick 1000Hz normal timers
|
||||
1 RCInput
|
||||
2 1000Hz Failsafe timer
|
||||
3,4 RCOut
|
||||
8 not used by AP
|
||||
SysTick 1000Hz normal timers
|
||||
1 CH1 RCInput
|
||||
2 CH1 1000Hz Failsafe timer
|
||||
3 CH1-4, 4 CH1-2 RCOut
|
||||
8 not used by AP
|
||||
|
||||
AP I2CDriver on Flymaple uses the libmaple Wire library, configured to clock
|
||||
at about 200kHz. The maximum possible Flymaple could support speed is limited
|
||||
|
@ -19,6 +19,7 @@
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "UARTDriver.h"
|
||||
#include "FlymapleWirish.h"
|
||||
@ -28,7 +29,9 @@
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
FLYMAPLEUARTDriver::FLYMAPLEUARTDriver(HardwareSerial* hws):
|
||||
_hws(hws)
|
||||
_hws(hws),
|
||||
_txBuf(NULL),
|
||||
_txBufSize(63) // libmaple usart default driver buffer of 63
|
||||
{}
|
||||
|
||||
void FLYMAPLEUARTDriver::begin(uint32_t b)
|
||||
@ -38,7 +41,17 @@ void FLYMAPLEUARTDriver::begin(uint32_t b)
|
||||
|
||||
void FLYMAPLEUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
|
||||
{
|
||||
begin(b);
|
||||
// Our private buffer can only grow, never shrink
|
||||
if (txS > _txBufSize)
|
||||
{
|
||||
if (_txBuf)
|
||||
free(_txBuf); // CAUTION: old contents lost
|
||||
_txBuf = (uint8_t*)malloc(txS);
|
||||
_txBufSize = txS;
|
||||
}
|
||||
begin(b); // libmaple internal ring buffer reinititalised to defaults here
|
||||
if (_txBuf)
|
||||
rb_init(_hws->c_dev()->rb, _txBufSize, _txBuf); // Get the ring buffer size we want
|
||||
}
|
||||
|
||||
void FLYMAPLEUARTDriver::end()
|
||||
|
@ -45,6 +45,8 @@ public:
|
||||
size_t write(uint8_t c);
|
||||
private:
|
||||
HardwareSerial* _hws;
|
||||
uint8_t* _txBuf; // If need more than libmaple usart driver buffer of 63
|
||||
uint16_t _txBufSize; // Allocated space in _txBuf
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_FLYMAPLE_UARTDRIVER_H__
|
||||
|
Loading…
Reference in New Issue
Block a user