rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

OP-1507 - Handles GPS data a chunk at time instead of a single byte at time.

Browse Source
This commit is contained in:
Alessio Morale 2014-09-27 00:17:00 +02:00
parent be747138e1
commit d0b37bab28
6 changed files with 165 additions and 136 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
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,6 +83,10 @@ void updateGpsSettings(UAVObjEvent *ev);
// the new location with Set = true.
#define GPS_HOMELOCATION_SET_DELAY 5000
#define GPS_LOOP_DELAY_MS 6
#define GPS_READ_BUFFER 128
#ifdef PIOS_GPS_SETS_HOMELOCATION
// Unfortunately need a good size stack for the WMM calculation
#define STACK_SIZE_BYTES 1024
@ -238,9 +246,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;
@ -253,12 +268,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)
@ -272,7 +291,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
#endif
res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
}
break;
#endif
@ -281,6 +300,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;
@ -327,6 +347,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
}
}
vTaskDelayUntil(&xLastWakeTime, GPS_LOOP_DELAY_MS / portTICK_RATE_MS);
}
}

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

@ -106,12 +106,16 @@ 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;
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;
@ -128,7 +132,7 @@ int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *Gps
start_flag = false;
found_cr = false;
rx_count = 0;
return PARSER_OVERRUN;
ret = PARSER_OVERRUN;
} else {
gps_rx_buffer[rx_count] = c;
rx_count++;
@ -161,7 +165,7 @@ int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *Gps
// PIOS_DEBUG_PinHigh(2);
gpsRxStats->gpsRxChkSumError++;
// PIOS_DEBUG_PinLow(2);
return PARSER_ERROR;
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);
@ -171,10 +175,11 @@ int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *Gps
gpsRxStats->gpsRxReceived++;
};
return PARSER_COMPLETE;
ret = PARSER_COMPLETE;
}
}
return PARSER_INCOMPLETE;
}
return ret;
}
static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)

15
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,11 +120,13 @@ 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;
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
@ -188,14 +191,14 @@ int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsD
}
if (proto_state == START) {
return PARSER_ERROR; // parser couldn't use this byte
ret = (ret != PARSER_COMPLETE) ? PARSER_ERROR : PARSER_COMPLETE; // parser couldn't use this byte
} else if (proto_state == FINISHED) {
gpsRxStats->gpsRxReceived++;
proto_state = START;
return PARSER_COMPLETE; // message complete & processed
ret = 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 */

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