rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-31 16:52:10 +01:00
Code Issues Releases Activity

Merged in TheOtherCliff/librepilot/theothercliff/LP-145_CC3D_GPS_issue (pull request #123)

Browse Source 
LP-145 CC3D GPS issue
This commit is contained in:
Lalanne Laurent 2016-02-16 19:13:10 +01:00
commit b3151602b2
4 changed files with 87 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -77,7 +77,9 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev);
// ****************
// Private constants
#define GPS_TIMEOUT_MS 500
// GPS timeout is greater than 1000ms so that a stock GPS configuration can be used without timeout errors
#define GPS_TIMEOUT_MS 1250
// delay from detecting HomeLocation.Set == False before setting new homelocation
// this prevent that a save with homelocation.Set = false triggered by gps ends saving
// the new location with Set = true.
@ -120,7 +122,6 @@ static xTaskHandle gpsTaskHandle;
static char *gps_rx_buffer;
static uint32_t timeOfLastCommandMs;
static uint32_t timeOfLastUpdateMs;
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER)
@ -256,8 +257,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
#endif
GPSPositionSensorData gpspositionsensor;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
timeOfLastUpdateMs = timeNowMs;
GPSPositionSensorGet(&gpspositionsensor);
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
@ -312,13 +312,14 @@ static void gpsTask(__attribute__((unused)) void *parameters)
#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
uint16_t cnt;
int res;
// This blocks the task until there is something on the buffer (or 100ms? passes)
cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
res = PARSER_INCOMPLETE;
if (cnt > 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:
@ -339,20 +340,23 @@ static void gpsTask(__attribute__((unused)) void *parameters)
if (res == PARSER_COMPLETE) {
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
}
}
// if there is any error at all, set status to NOGPS
// Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS ||
if ((res == PARSER_ERROR) ||
(timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS ||
(gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
// we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS;
GPSPositionSensorStatusSet(&status);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
} else {
}
// if we parsed at least one packet successfully
else if (res == PARSER_COMPLETE) {
// we appear to be receiving GPS sentences OK, we've had an update
// criteria for GPS-OK taken from this post...
// http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
@ -547,7 +551,7 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
load_mag_settings();
op_gpsv9_load_mag_settings();
}

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

@ -107,7 +107,6 @@ static const struct nmea_parser nmea_parsers[] = {
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;
@ -139,8 +138,6 @@ int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPosition
} else {
i = len;
}
// if no more data, we can return an error
ret = PARSER_ERROR;
// loop to restart at the $ if there is one
continue;
}
@ -150,7 +147,6 @@ int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPosition
// Flush the buffer and note the overflow event.
gpsRxStats->gpsRxOverflow++;
start_flag = false;
ret = PARSER_OVERRUN;
continue;
} else {
gps_rx_buffer[rx_count++] = c;
@ -182,13 +178,11 @@ int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPosition
// 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);
ret = PARSER_ERROR;
} else {
gpsRxStats->gpsRxReceived++;
goodParse = true;
@ -205,7 +199,7 @@ int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPosition
// might think the GPS was offline
return PARSER_COMPLETE;
} else {
return ret;
return PARSER_INCOMPLETE;
}
}

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

@ -122,49 +122,70 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
UBX_CHK2,
FINISHED
};
enum restart_states {
RESTART_WITH_ERROR,
RESTART_NO_ERROR
};
uint8_t c;
static enum proto_states proto_state = START;
static uint16_t rx_count = 0;
struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
int i = 0;
uint16_t i = 0;
uint16_t restart_index = 0;
enum restart_states restart_state;
// switch continue is the normal condition and comes back to here for another byte
// switch break is the error state that branches to the end and restarts the scan at the byte after the first sync byte
while (i < len) {
c = rx[i++];
switch (proto_state) {
case START: // detect protocol
if (c == UBX_SYNC1) { // first UBX sync char found
proto_state = UBX_SY2;
proto_state = UBX_SY2;
// restart here, at byte after SYNC1, if we fail to parse
restart_index = i;
}
break;
continue;
case UBX_SY2:
if (c == UBX_SYNC2) { // second UBX sync char found
proto_state = UBX_CLASS;
} else {
proto_state = START; // reset state
restart_state = RESTART_NO_ERROR;
break;
}
break;
continue;
case UBX_CLASS:
ubx->header.class = c;
proto_state = UBX_ID;
break;
continue;
case UBX_ID:
ubx->header.id = c;
proto_state = UBX_LEN1;
break;
continue;
case UBX_LEN1:
ubx->header.len = c;
proto_state = UBX_LEN2;
break;
continue;
case UBX_LEN2:
ubx->header.len += (c << 8);
if (ubx->header.len > sizeof(UBXPayload)) {
gpsRxStats->gpsRxOverflow++;
proto_state = START;
#if defined(PIOS_GPS_MINIMAL)
restart_state = RESTART_NO_ERROR;
break;
#else
restart_state = RESTART_WITH_ERROR;
break;
#endif
} else {
rx_count = 0;
proto_state = UBX_PAYLOAD;
if (ubx->header.len == 0) {
proto_state = UBX_CHK1;
} else {
proto_state = UBX_PAYLOAD;
rx_count = 0;
}
}
break;
continue;
case UBX_PAYLOAD:
if (rx_count < ubx->header.len) {
ubx->payload.payload[rx_count] = c;
@ -172,32 +193,50 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
proto_state = UBX_CHK1;
}
}
break;
continue;
case UBX_CHK1:
ubx->header.ck_a = c;
proto_state = UBX_CHK2;
break;
continue;
case UBX_CHK2:
ubx->header.ck_b = c;
// OP GPSV9 sends data with bad checksums this appears to happen because it drops data
// this has been proven by running it without autoconfig and testing:
// data coming from OPV9 "GPS+MCU" port the checksum errors happen roughly every 5 to 30 seconds
// same data coming from OPV9 "GPS Only" port the checksums are always good
// this also ocassionally causes parse_ubx_message() to issue alarms because not all the messages were received
// see OP GPSV9 comment in parse_ubx_message() for further information
if (checksum_ubx_message(ubx)) { // message complete and valid
parse_ubx_message(ubx, GpsData);
proto_state = FINISHED;
gpsRxStats->gpsRxReceived++;
proto_state = START;
// pass PARSER_ERROR to be to caller if it happens even once
if (ret == PARSER_INCOMPLETE) {
ret = PARSER_COMPLETE; // message complete & processed
}
} else {
gpsRxStats->gpsRxChkSumError++;
proto_state = START;
restart_state = RESTART_WITH_ERROR;
break;
}
break;
continue;
default:
break;
continue;
}
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
// this simple restart doesn't work across calls
// but it does work within a single call
// and it does the expected thing across calls
// if restarting due to error detected in 2nd call to this function (on split packet)
// then we just restart at index 0, which is mid-packet, not the second byte
if (restart_state == RESTART_WITH_ERROR) {
ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets)
}
// else restart with no error
rx += restart_index; // restart parsing just past the most recent SYNC1
len -= restart_index;
i = 0;
proto_state = START;
}
return ret;
@ -528,7 +567,7 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
GpsPosition->SensorType = sensorType;
if (msgtracker.msg_received == ALL_RECEIVED) {
// leave my new field alone!
// leave BaudRate field alone!
GPSPositionSensorBaudRateGet(&GpsPosition->BaudRate);
GPSPositionSensorSet(GpsPosition);
msgtracker.msg_received = NONE_RECEIVED;
@ -538,6 +577,10 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
GPSPositionSensorStatusGet(&status);
if (status == GPSPOSITIONSENSOR_STATUS_NOGPS) {
// Some ubx thing has been received so GPS is there
//
// OP GPSV9 will sometimes cause this NOFIX
// because GPSV9 drops data which causes checksum errors which causes GPS.c to set the status to NOGPS
// see OP GPSV9 comment in parse_ubx_stream() for further information
status = GPSPOSITIONSENSOR_STATUS_NOFIX;
GPSPositionSensorStatusSet(&status);
}
@ -546,18 +589,12 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
}
#if !defined(PIOS_GPS_MINIMAL)
void load_mag_settings()
void op_gpsv9_load_mag_settings()
{
auxmagsupport_reload_settings();
if (auxmagsupport_get_type() != AUXMAGSETTINGS_TYPE_GPSV9) {
useMag = false;
const uint8_t status = AUXMAGSENSOR_STATUS_NONE;
// next sample from other external mags will provide the right status if present
AuxMagSensorStatusSet((uint8_t *)&status);
} else {
if (auxmagsupport_get_type() == AUXMAGSETTINGS_TYPE_GPSV9) {
useMag = true;
} else {
useMag = false;
}
}
#endif

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

@ -620,6 +620,6 @@ bool checksum_ubx_message(struct UBXPacket *);
uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
int parse_ubx_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
void load_mag_settings();
void op_gpsv9_load_mag_settings();
#endif /* UBX_H */