rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merged in hwhankins/librepilot/LP-252_GPSv9_drops_packets (pull request #193)

Browse Source 
Lp 252_gpsv9_drops_packets
This commit is contained in:
James Duley 2016-03-10 22:00:21 +00:00
commit d6616750b6
3 changed files with 26 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
flight/modules/gpsp/gps9gpshandler.c
View File

@ -34,41 +34,42 @@
#include "gps9protocol.h" #include "gps9protocol.h"
uint32_t lastUnsentData = 0; uint32_t lastUnsentData = 0;
uint8_t buffer[BUFFER_SIZE]; uint8_t gpsBuffer[GPS_BUFFER_SIZE];
uint8_t fcBuffer[FC_BUFFER_SIZE];
void handleGPS() void handleGPS()
{ {
bool completeSentenceSent = false; bool completeSentenceInBuffer = false;
int8_t maxCount = 2; int8_t maxCount = 2;
do { do {
int32_t datacounter = PIOS_UBX_DDC_GetAvailableBytes(PIOS_I2C_GPS); int32_t datacounter = PIOS_UBX_DDC_GetAvailableBytes(PIOS_I2C_GPS);
if (datacounter > 0) { if (datacounter > 0) {
uint8_t toRead = (uint32_t)datacounter > BUFFER_SIZE - lastUnsentData ? BUFFER_SIZE - lastUnsentData : (uint8_t)datacounter; uint8_t toRead = (uint32_t)datacounter > GPS_BUFFER_SIZE - lastUnsentData ? GPS_BUFFER_SIZE - lastUnsentData : (uint8_t)datacounter;
uint8_t toSend = toRead; uint8_t toSend = toRead + lastUnsentData;
PIOS_UBX_DDC_ReadData(PIOS_I2C_GPS, buffer, toRead); PIOS_UBX_DDC_ReadData(PIOS_I2C_GPS, gpsBuffer + lastUnsentData, toRead);
uint8_t *lastSentence; uint8_t *lastSentence;
uint16_t lastSentenceLength; uint16_t lastSentenceLength;
completeSentenceSent = ubx_getLastSentence(buffer, toRead, &lastSentence, &lastSentenceLength); completeSentenceInBuffer = ubx_getLastSentence(gpsBuffer, toSend, &lastSentence, &lastSentenceLength);
if (completeSentenceSent) { if (completeSentenceInBuffer) {
toSend = (uint8_t)(lastSentence - buffer + lastSentenceLength); toSend = (uint8_t)(lastSentence - gpsBuffer + lastSentenceLength);
PIOS_COM_SendBuffer(pios_com_main_id, gpsBuffer, toSend);
// move unsent data to the beginning of gpsBuffer to be sent next time
lastUnsentData = lastUnsentData > toSend ? lastUnsentData - toSend : 0;
if (lastUnsentData > 0) {
memcpy(gpsBuffer, (gpsBuffer + toSend), lastUnsentData);
}
} else { } else {
lastUnsentData = 0; // toss the buffer contents if it's full and there are no complete
} // ublox sentences in it. This happens if the module is sending NMEA
lastUnsentData = toSend < GPS_BUFFER_SIZE ? toSend : 0;
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;
memcpy(buffer, (buffer + toSend), lastUnsentData);
} }
} }
datacounter = PIOS_COM_ReceiveBuffer(pios_com_main_id, buffer, BUFFER_SIZE, 0); datacounter = PIOS_COM_ReceiveBuffer(pios_com_main_id, fcBuffer, FC_BUFFER_SIZE, 0);
if (datacounter > 0) { if (datacounter > 0) {
PIOS_UBX_DDC_WriteData(PIOS_I2C_GPS, buffer, datacounter); PIOS_UBX_DDC_WriteData(PIOS_I2C_GPS, fcBuffer, datacounter);
} }
if (maxCount) { if (maxCount) {
// Note: this delay is needed as querying too quickly the UBX module's I2C(DDC) // Note: this delay is needed as querying too quickly the UBX module's I2C(DDC)

4
flight/modules/gpsp/gpsdsysmod.c
View File

@ -110,6 +110,10 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters)
while (!initTaskDone) { while (!initTaskDone) {
vTaskDelay(10); vTaskDelay(10);
} }
#ifndef PIOS_INCLUDE_WDG
// if no watchdog is enabled, don't reset watchdog in MODULE_TASKCREATE_ALL loop
#define PIOS_WDG_Clear()
#endif
/* create all modules thread */ /* create all modules thread */
MODULE_TASKCREATE_ALL; MODULE_TASKCREATE_ALL;

3
flight/modules/gpsp/inc/gps9gpshandler.h
View File

@ -26,7 +26,8 @@
#ifndef GPS9GPSHANDLER_H_ #ifndef GPS9GPSHANDLER_H_
#define GPS9GPSHANDLER_H_ #define GPS9GPSHANDLER_H_
#define BUFFER_SIZE 200 #define GPS_BUFFER_SIZE 250
#define FC_BUFFER_SIZE 120
void handleGPS(); void handleGPS();
void setupGPS(); void setupGPS();