mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
OP-1477 - Fix issue with quick request to DDC port, change baud rate to 57600, fix sys rate, optimize uart tx
This commit is contained in:
parent
5054bda433
commit
0ffb99f228
@ -39,12 +39,13 @@ uint8_t buffer[BUFFER_SIZE];
|
||||
void handleGPS()
|
||||
{
|
||||
bool completeSentenceSent = false;
|
||||
int8_t maxCount = 3;
|
||||
int8_t maxCount = 2;
|
||||
|
||||
do {
|
||||
int32_t datacounter = PIOS_UBX_DDC_GetAvailableBytes(PIOS_I2C_GPS);
|
||||
if (datacounter > 0) {
|
||||
uint8_t toRead = (uint32_t)datacounter > BUFFER_SIZE - lastUnsentData ? BUFFER_SIZE - lastUnsentData : (uint8_t)datacounter;
|
||||
//uint8_t toRead = (uint32_t)datacounter > BUFFER_SIZE ? BUFFER_SIZE : (uint8_t)datacounter;
|
||||
uint8_t toSend = toRead;
|
||||
PIOS_UBX_DDC_ReadData(PIOS_I2C_GPS, buffer, toRead);
|
||||
|
||||
@ -58,6 +59,7 @@ void handleGPS()
|
||||
}
|
||||
|
||||
PIOS_COM_SendBuffer(pios_com_main_id, buffer, toSend);
|
||||
|
||||
if (toRead > toSend) {
|
||||
// move unsent data at the beginning of buffer to be sent next time
|
||||
lastUnsentData = toRead - toSend;
|
||||
@ -69,6 +71,11 @@ void handleGPS()
|
||||
if (datacounter > 0) {
|
||||
PIOS_UBX_DDC_WriteData(PIOS_I2C_GPS, buffer, datacounter);
|
||||
}
|
||||
if(maxCount){
|
||||
// Note: this delay is needed as querying too quickly the UBX module's I2C(DDC)
|
||||
// port causes a lot of weird issues (it stops sending nav sentences)
|
||||
vTaskDelay(2 * configTICK_RATE_HZ / 1000);
|
||||
}
|
||||
} while (maxCount--);
|
||||
}
|
||||
|
||||
@ -87,7 +94,7 @@ void setupGPS()
|
||||
cfgprt.fragments.data.txReady = CFG_PRT_DATA_TXREADI_DISABLED;
|
||||
cfgprt.fragments.data.mode = CFG_PRT_DATA_MODE_ADDR;
|
||||
cfgprt.fragments.data.reserved3 = 0;
|
||||
cfgprt.fragments.data.inProtoMask = CFG_PRT_DATA_PROTO_UBX | CFG_PRT_DATA_PROTO_RTCM;
|
||||
cfgprt.fragments.data.inProtoMask = CFG_PRT_DATA_PROTO_UBX | CFG_PRT_DATA_PROTO_NMEA |CFG_PRT_DATA_PROTO_RTCM;
|
||||
cfgprt.fragments.data.outProtoMask = CFG_PRT_DATA_PROTO_UBX;
|
||||
cfgprt.fragments.data.flags = 0;
|
||||
cfgprt.fragments.data.reserved5 = 0;
|
||||
|
@ -119,24 +119,21 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters)
|
||||
#if defined(PIOS_INCLUDE_IAP)
|
||||
PIOS_IAP_WriteBootCount(0);
|
||||
#endif
|
||||
/* Right now there is no configuration and uart speed is fixed at 115200.
|
||||
/* Right now there is no configuration and uart speed is fixed at 57600.
|
||||
* TODO:
|
||||
* 1) add a tiny ubx parser on gps side to intercept CFG-RINV and use that for config storage;
|
||||
* 2) second ubx parser on uart side that intercept custom configuration message and flash commands.
|
||||
*/
|
||||
PIOS_COM_ChangeBaud(pios_com_main_id, 115200);
|
||||
static TickType_t lastUpdate;
|
||||
PIOS_COM_ChangeBaud(pios_com_main_id, GPS_MODULE_DEFAULT_BAUDRATE);
|
||||
setupGPS();
|
||||
uint32_t ledTimer = 0;
|
||||
|
||||
static TickType_t lastUpdate;
|
||||
readFirmwareInfo();
|
||||
|
||||
while (1) {
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_SYSTEM);
|
||||
#endif
|
||||
// NotificationUpdateStatus();
|
||||
// Update the system statistics
|
||||
uint32_t ledPeriod = PIOS_DELAY_DiffuS(ledTimer) / 1000;
|
||||
if (ledPeriod < HB_LED_BLINK_ON_PERIOD_MS) {
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
|
||||
@ -147,11 +144,10 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters)
|
||||
ledTimer = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
|
||||
vTaskDelayUntil(&lastUpdate, SYSTEM_UPDATE_PERIOD_MS * configTICK_RATE_HZ / 1000);
|
||||
|
||||
handleGPS();
|
||||
handleMag();
|
||||
updateStats();
|
||||
vTaskDelayUntil(&lastUpdate, SYSTEM_UPDATE_PERIOD_MS * configTICK_RATE_HZ / 1000);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -34,6 +34,8 @@
|
||||
#include <pios_helpers.h>
|
||||
#include <ubx_utils.h>
|
||||
|
||||
#define GPS_MODULE_DEFAULT_BAUDRATE 57600
|
||||
|
||||
int32_t GPSPSystemModInitialize(void);
|
||||
|
||||
#endif // GPSSYSTEMMOD_H
|
||||
|
@ -34,6 +34,7 @@
|
||||
|
||||
#include <pios_usart_priv.h>
|
||||
|
||||
#define PIOS_UART_TX_BUFFER 10
|
||||
/* Provide a COM driver */
|
||||
static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud);
|
||||
static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context);
|
||||
@ -63,6 +64,10 @@ struct pios_usart_dev {
|
||||
uint32_t tx_out_context;
|
||||
|
||||
uint32_t rx_dropped;
|
||||
|
||||
uint8_t tx_buffer[PIOS_UART_TX_BUFFER];
|
||||
uint8_t tx_len;
|
||||
uint8_t tx_pos;
|
||||
};
|
||||
|
||||
static struct pios_usart_dev *PIOS_USART_validate(uint32_t usart_id)
|
||||
@ -283,14 +288,16 @@ static void PIOS_USART_generic_irq_handler(uint32_t usart_id)
|
||||
bool tx_need_yield = false;
|
||||
if (USART_GetITStatus(usart_dev->cfg->regs, USART_IT_TXE) != RESET) {
|
||||
if (usart_dev->tx_out_cb) {
|
||||
uint8_t b;
|
||||
uint16_t bytes_to_send;
|
||||
|
||||
bytes_to_send = (usart_dev->tx_out_cb)(usart_dev->tx_out_context, &b, 1, NULL, &tx_need_yield);
|
||||
|
||||
if (bytes_to_send > 0) {
|
||||
//uint8_t b;
|
||||
//uint16_t bytes_to_send;
|
||||
if(!usart_dev->tx_len){
|
||||
usart_dev->tx_len = (usart_dev->tx_out_cb)(usart_dev->tx_out_context, usart_dev->tx_buffer, PIOS_UART_TX_BUFFER, NULL, &tx_need_yield);
|
||||
usart_dev->tx_pos = 0;
|
||||
}
|
||||
if (usart_dev->tx_len > 0) {
|
||||
/* Send the byte we've been given */
|
||||
USART_SendData(usart_dev->cfg->regs, b);
|
||||
USART_SendData(usart_dev->cfg->regs, usart_dev->tx_buffer[usart_dev->tx_pos++]);
|
||||
usart_dev->tx_len--;
|
||||
} else {
|
||||
/* No bytes to send, disable TXE interrupt */
|
||||
USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE);
|
||||
|
Loading…
x
Reference in New Issue
Block a user