rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

Merge branch 'amorale/OP-1507_gps_module_cpu_optimization' into next

Browse Source
This commit is contained in:
Alessio Morale 2014-10-03 00:37:39 +02:00
commit 9cfd55d006
8 changed files with 207 additions and 160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
flight/libraries/fifo_buffer.c
View File

@ -148,14 +148,18 @@ uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len)
uint16_t i = 0;
while (num_bytes > 0) {
uint16_t j = buf_size - rd;
if (j > num_bytes) {
j = num_bytes;
uint16_t block_len = buf_size - rd;
if (block_len > num_bytes) {
block_len = num_bytes;
}
memcpy(p + i, buff + rd, j);
i += j;
num_bytes -= j;
rd += j;
if (block_len == 1) {
*((uint8_t *)(p + i)) = *((uint8_t *)(buff + rd));
} else {
memcpy(p + i, buff + rd, block_len);
}
i += block_len;
num_bytes -= block_len;
rd += block_len;
if (rd >= buf_size) {
rd = 0;
}
@ -184,14 +188,18 @@ uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len)
uint16_t i = 0;
while (num_bytes > 0) {
uint16_t j = buf_size - rd;
if (j > num_bytes) {
j = num_bytes;
uint16_t block_len = buf_size - rd;
if (block_len > num_bytes) {
block_len = num_bytes;
}
memcpy(p + i, buff + rd, j);
i += j;
num_bytes -= j;
rd += j;
if (block_len == 1) {
*((uint8_t *)(p + i)) = *((uint8_t *)(buff + rd));
} else {
memcpy(p + i, buff + rd, block_len);
}
i += block_len;
num_bytes -= block_len;
rd += block_len;
if (rd >= buf_size) {
rd = 0;
}
@ -243,14 +251,18 @@ uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len)
uint16_t i = 0;
while (num_bytes > 0) {
uint16_t j = buf_size - wr;
if (j > num_bytes) {
j = num_bytes;
uint16_t block_len = buf_size - wr;
if (block_len > num_bytes) {
block_len = num_bytes;
}
memcpy(buff + wr, p + i, j);
i += j;
num_bytes -= j;
wr += j;
if (block_len == 1) {
*((uint8_t *)(buff + wr)) = *((uint8_t *)(p + i));
} else {
memcpy(buff + wr, p + i, block_len);
}
i += block_len;
num_bytes -= block_len;
wr += block_len;
if (wr >= buf_size) {
wr = 0;
}

32
flight/modules/GPS/GPS.c
View File

@ -52,6 +52,10 @@
#include "inc/ubx_autoconfig.h"
#endif
#include <pios_instrumentation_helper.h>
PERF_DEFINE_COUNTER(counterBytesIn);
PERF_DEFINE_COUNTER(counterRate);
PERF_DEFINE_COUNTER(counterParse);
// ****************
// Private functions
@ -79,17 +83,24 @@ void updateGpsSettings(UAVObjEvent *ev);
// the new location with Set = true.
#define GPS_HOMELOCATION_SET_DELAY 5000
#define GPS_LOOP_DELAY_MS 6
#ifdef PIOS_GPS_SETS_HOMELOCATION
// Unfortunately need a good size stack for the WMM calculation
#define STACK_SIZE_BYTES 1024
#else
#if defined(PIOS_GPS_MINIMAL)
#define GPS_READ_BUFFER 32
#define STACK_SIZE_BYTES 500
#else
#define STACK_SIZE_BYTES 650
#endif // PIOS_GPS_MINIMAL
#endif // PIOS_GPS_SETS_HOMELOCATION
#ifndef GPS_READ_BUFFER
#define GPS_READ_BUFFER 128
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
// ****************
@ -238,9 +249,16 @@ static void gpsTask(__attribute__((unused)) void *parameters)
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
updateGpsSettings(0);
#endif
TickType_t xLastWakeTime;
xLastWakeTime = xTaskGetTickCount();
PERF_INIT_COUNTER(counterBytesIn, 0x97510001);
PERF_INIT_COUNTER(counterRate, 0x97510002);
PERF_INIT_COUNTER(counterParse, 0x97510003);
uint8_t c[GPS_READ_BUFFER];
// Loop forever
while (1) {
uint8_t c;
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
char *buffer = 0;
@ -255,12 +273,16 @@ static void gpsTask(__attribute__((unused)) void *parameters)
}
#endif
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) {
uint16_t cnt;
while ((cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay)) > 0) {
PERF_TIMED_SECTION_START(counterParse);
PERF_TRACK_VALUE(counterBytesIn, cnt);
PERF_MEASURE_PERIOD(counterRate);
int res;
switch (gpsSettings.DataProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case GPSSETTINGS_DATAPROTOCOL_NMEA:
res = parse_nmea_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
break;
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
@ -280,7 +302,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
lastStatus = gpspositionsensor.AutoConfigStatus;
}
#endif
res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
}
break;
#endif
@ -289,6 +311,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
break;
}
PERF_TIMED_SECTION_END(counterParse);
if (res == PARSER_COMPLETE) {
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs;
@ -336,6 +359,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
}
}
vTaskDelayUntil(&xLastWakeTime, GPS_LOOP_DELAY_MS / portTICK_RATE_MS);
}
}

125
flight/modules/GPS/NMEA.c
View File

@ -106,75 +106,80 @@ static const struct nmea_parser nmea_parsers[] = {
#endif // PIOS_GPS_MINIMAL
};
int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
{
int ret = PARSER_INCOMPLETE;
static uint8_t rx_count = 0;
static bool start_flag = false;
static bool found_cr = false;
uint8_t c;
// detect start while acquiring stream
if (!start_flag && (c == '$')) { // NMEA identifier found
start_flag = true;
found_cr = false;
rx_count = 0;
} else if (!start_flag) {
return PARSER_ERROR;
}
if (rx_count >= NMEA_MAX_PACKET_LENGTH) {
// The buffer is already full and we haven't found a valid NMEA sentence.
// Flush the buffer and note the overflow event.
gpsRxStats->gpsRxOverflow++;
start_flag = false;
found_cr = false;
rx_count = 0;
return PARSER_OVERRUN;
} else {
gps_rx_buffer[rx_count] = c;
rx_count++;
}
// look for ending '\r\n' sequence
if (!found_cr && (c == '\r')) {
found_cr = true;
} else if (found_cr && (c != '\n')) {
found_cr = false; // false end flag
} else if (found_cr && (c == '\n')) {
// The NMEA functions require a zero-terminated string
// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
gps_rx_buffer[rx_count - 2] = 0;
// prepare to parse next sentence
start_flag = false;
found_cr = false;
rx_count = 0;
// Our rxBuffer must look like this now:
// [0] = '$'
// ... = zero or more bytes of sentence payload
// [end_pos - 1] = '\r'
// [end_pos] = '\n'
//
// Prepare to consume the sentence from the buffer
// Validate the checksum over the sentence
if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum. May indicate dropped characters on Rx.
// PIOS_DEBUG_PinHigh(2);
gpsRxStats->gpsRxChkSumError++;
// PIOS_DEBUG_PinLow(2);
for (int i = 0; i < len; i++) {
c = rx[i];
// detect start while acquiring stream
if (!start_flag && (c == '$')) { // NMEA identifier found
start_flag = true;
found_cr = false;
rx_count = 0;
} else if (!start_flag) {
return PARSER_ERROR;
} else { // Valid checksum, use this packet to update the GPS position
if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
// PIOS_DEBUG_PinHigh(2);
gpsRxStats->gpsRxParserError++;
// PIOS_DEBUG_PinLow(2);
} else {
gpsRxStats->gpsRxReceived++;
};
}
return PARSER_COMPLETE;
if (rx_count >= NMEA_MAX_PACKET_LENGTH) {
// The buffer is already full and we haven't found a valid NMEA sentence.
// Flush the buffer and note the overflow event.
gpsRxStats->gpsRxOverflow++;
start_flag = false;
found_cr = false;
rx_count = 0;
ret = PARSER_OVERRUN;
} else {
gps_rx_buffer[rx_count] = c;
rx_count++;
}
// look for ending '\r\n' sequence
if (!found_cr && (c == '\r')) {
found_cr = true;
} else if (found_cr && (c != '\n')) {
found_cr = false; // false end flag
} else if (found_cr && (c == '\n')) {
// The NMEA functions require a zero-terminated string
// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
gps_rx_buffer[rx_count - 2] = 0;
// prepare to parse next sentence
start_flag = false;
found_cr = false;
rx_count = 0;
// Our rxBuffer must look like this now:
// [0] = '$'
// ... = zero or more bytes of sentence payload
// [end_pos - 1] = '\r'
// [end_pos] = '\n'
//
// Prepare to consume the sentence from the buffer
// Validate the checksum over the sentence
if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum. May indicate dropped characters on Rx.
// PIOS_DEBUG_PinHigh(2);
gpsRxStats->gpsRxChkSumError++;
// PIOS_DEBUG_PinLow(2);
ret = PARSER_ERROR;
} else { // Valid checksum, use this packet to update the GPS position
if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
// PIOS_DEBUG_PinHigh(2);
gpsRxStats->gpsRxParserError++;
// PIOS_DEBUG_PinLow(2);
} else {
gpsRxStats->gpsRxReceived++;
};
ret = PARSER_COMPLETE;
}
}
}
return PARSER_INCOMPLETE;
return ret;
}
static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)

141
flight/modules/GPS/UBX.c
View File

@ -105,8 +105,9 @@ struct UBX_ACK_NAK ubxLastNak;
#define UBX_PVT_TIMEOUT (1000)
// parse incoming character stream for messages in UBX binary format
int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
int parse_ubx_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
{
int ret = PARSER_INCOMPLETE; // message not (yet) complete
enum proto_states {
START,
UBX_SY2,
@ -119,83 +120,85 @@ int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsD
UBX_CHK2,
FINISHED
};
uint8_t c;
static enum proto_states proto_state = START;
static uint8_t rx_count = 0;
struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
switch (proto_state) {
case START: // detect protocol
if (c == UBX_SYNC1) { // first UBX sync char found
proto_state = UBX_SY2;
}
break;
case UBX_SY2:
if (c == UBX_SYNC2) { // second UBX sync char found
proto_state = UBX_CLASS;
} else {
proto_state = START; // reset state
}
break;
case UBX_CLASS:
ubx->header.class = c;
proto_state = UBX_ID;
break;
case UBX_ID:
ubx->header.id = c;
proto_state = UBX_LEN1;
break;
case UBX_LEN1:
ubx->header.len = c;
proto_state = UBX_LEN2;
break;
case UBX_LEN2:
ubx->header.len += (c << 8);
if (ubx->header.len > sizeof(UBXPayload)) {
gpsRxStats->gpsRxOverflow++;
proto_state = START;
} else {
rx_count = 0;
proto_state = UBX_PAYLOAD;
}
break;
case UBX_PAYLOAD:
if (rx_count < ubx->header.len) {
ubx->payload.payload[rx_count] = c;
if (++rx_count == ubx->header.len) {
proto_state = UBX_CHK1;
for (int i = 0; i < len; i++) {
c = rx[i];
switch (proto_state) {
case START: // detect protocol
if (c == UBX_SYNC1) { // first UBX sync char found
proto_state = UBX_SY2;
}
} else {
gpsRxStats->gpsRxOverflow++;
proto_state = START;
break;
case UBX_SY2:
if (c == UBX_SYNC2) { // second UBX sync char found
proto_state = UBX_CLASS;
} else {
proto_state = START; // reset state
}
break;
case UBX_CLASS:
ubx->header.class = c;
proto_state = UBX_ID;
break;
case UBX_ID:
ubx->header.id = c;
proto_state = UBX_LEN1;
break;
case UBX_LEN1:
ubx->header.len = c;
proto_state = UBX_LEN2;
break;
case UBX_LEN2:
ubx->header.len += (c << 8);
if (ubx->header.len > sizeof(UBXPayload)) {
gpsRxStats->gpsRxOverflow++;
proto_state = START;
} else {
rx_count = 0;
proto_state = UBX_PAYLOAD;
}
break;
case UBX_PAYLOAD:
if (rx_count < ubx->header.len) {
ubx->payload.payload[rx_count] = c;
if (++rx_count == ubx->header.len) {
proto_state = UBX_CHK1;
}
} else {
gpsRxStats->gpsRxOverflow++;
proto_state = START;
}
break;
case UBX_CHK1:
ubx->header.ck_a = c;
proto_state = UBX_CHK2;
break;
case UBX_CHK2:
ubx->header.ck_b = c;
if (checksum_ubx_message(ubx)) { // message complete and valid
parse_ubx_message(ubx, GpsData);
proto_state = FINISHED;
} else {
gpsRxStats->gpsRxChkSumError++;
proto_state = START;
}
break;
default: break;
}
break;
case UBX_CHK1:
ubx->header.ck_a = c;
proto_state = UBX_CHK2;
break;
case UBX_CHK2:
ubx->header.ck_b = c;
if (checksum_ubx_message(ubx)) { // message complete and valid
parse_ubx_message(ubx, GpsData);
proto_state = FINISHED;
} else {
gpsRxStats->gpsRxChkSumError++;
if (proto_state == START) {
ret = (ret != PARSER_COMPLETE) ? PARSER_ERROR : PARSER_COMPLETE; // parser couldn't use this byte
} else if (proto_state == FINISHED) {
gpsRxStats->gpsRxReceived++;
proto_state = START;
ret = PARSER_COMPLETE; // message complete & processed
}
break;
default: break;
}
if (proto_state == START) {
return PARSER_ERROR; // parser couldn't use this byte
} else if (proto_state == FINISHED) {
gpsRxStats->gpsRxReceived++;
proto_state = START;
return PARSER_COMPLETE; // message complete & processed
}
return PARSER_INCOMPLETE; // message not (yet) complete
return ret;
}

2
flight/modules/GPS/inc/NMEA.h
View File

@ -39,6 +39,6 @@
extern bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData);
extern bool NMEA_checksum(char *nmea_sentence);
extern int parse_nmea_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
extern int parse_nmea_stream(uint8_t *, uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
#endif /* NMEA_H */

2
flight/modules/GPS/inc/UBX.h
View File

@ -400,7 +400,7 @@ extern struct UBX_ACK_NAK ubxLastNak;
bool checksum_ubx_message(struct UBXPacket *);
uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
int parse_ubx_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
int parse_ubx_stream(uint8_t *rx, uint8_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
void load_mag_settings();
#endif /* UBX_H */

9
flight/pios/common/pios_com.c
View File

@ -207,9 +207,12 @@ static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t b
PIOS_Assert(valid);
PIOS_Assert(com_dev->has_rx);
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len);
uint16_t bytes_into_fifo;
if (buf_len == 1) {
bytes_into_fifo = fifoBuf_putByte(&com_dev->rx, buf[0]);
} else {
bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len);
}
if (bytes_into_fifo > 0) {
/* Data has been added to the buffer */
PIOS_COM_UnblockRx(com_dev, need_yield);

2
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -216,7 +216,7 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512
#define PIOS_COM_GPS_RX_BUF_LEN 32
#define PIOS_COM_GPS_RX_BUF_LEN 128
#define PIOS_COM_GPS_TX_BUF_LEN 32
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65