rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GPS - Add packet logging to the OLED display. Allow autobauding to be 

enabled/disabled via cli (disabled by default now).  Fix missing rate
configuration for UBLOX SVINFO which would have resulted in missing
satallite counts.
This commit is contained in:
Dominic Clifton 2014-12-12 14:43:59 +00:00
parent 8345401ff2
commit 6ce288063e
5 changed files with 113 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/main/config/config.c
View File

@ -104,7 +104,7 @@ profile_t *currentProfile;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 85;
static const uint8_t EEPROM_CONF_VERSION = 86;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -364,7 +364,8 @@ static void resetConf(void)
// gps/nav stuff
masterConfig.gpsConfig.provider = GPS_NMEA;
masterConfig.gpsConfig.sbasMode = SBAS_AUTO;
masterConfig.gpsConfig.gpsAutoConfig = GPS_AUTOCONFIG_ON;
masterConfig.gpsConfig.autoConfig = GPS_AUTOCONFIG_ON;
masterConfig.gpsConfig.autoBaud = GPS_AUTOBAUD_OFF;
#endif
resetSerialConfig(&masterConfig.serialConfig);

40
src/main/io/display.c
View File

@ -302,11 +302,6 @@ void showGpsPage() {
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "Errors: %d", gpsData.errors);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "Lat: %d, Lon: %d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
@ -317,15 +312,27 @@ void showGpsPage() {
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
#ifdef GPS_PH_DEBUG
tfp_sprintf(lineBuffer, "Angles: P:%d, R:%d", GPS_angle[PITCH], GPS_angle[ROLL]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
#endif
tfp_sprintf(lineBuffer, "%d cm/s, gc: %d", GPS_speed, GPS_ground_course);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "Errors: %d", gpsData.errors);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "%16s", gpsPacketLog);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}
void showBatteryPage(void)
@ -475,6 +482,12 @@ void updateDisplay(void)
}
}
void displaySetPage(pageId_e pageId)
{
pageState.pageId = pageId;
pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
}
void displayInit(rxConfig_t *rxConfigToUse)
{
delay(200);
@ -484,29 +497,32 @@ void displayInit(rxConfig_t *rxConfigToUse)
rxConfig = rxConfigToUse;
memset(&pageState, 0, sizeof(pageState));
pageState.pageId = PAGE_WELCOME;
displaySetPage(PAGE_WELCOME);
updateDisplay();
displaySetNextPageChangeAt(micros() + (1000 * 1000 * 5));
}
void displayShowFixedPage(void) {
pageState.pageId = PAGE_GPS;
pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
void displayShowFixedPage(void)
{
displaySetPage(PAGE_GPS);
displayDisablePageCycling();
}
void displaySetNextPageChangeAt(uint32_t futureMicros) {
void displaySetNextPageChangeAt(uint32_t futureMicros)
{
pageState.nextPageAt = futureMicros;
}
void displayEnablePageCycling(void) {
void displayEnablePageCycling(void)
{
pageState.pageFlags |= PAGE_STATE_FLAG_CYCLE_ENABLED;
pageState.cycleIndex = CYCLE_PAGE_ID_COUNT - 1; // start at first page
}
void displayDisablePageCycling(void) {
void displayDisablePageCycling(void)
{
pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED;
}

95
src/main/io/gps.c
View File

@ -52,7 +52,18 @@
extern int16_t debug[4];
#define LOG_NMEA_GGA 'g'
#define LOG_NMEA_RMC 'r'
#define LOG_UBLOX_SOL 'O'
#define LOG_UBLOX_STATUS 'S'
#define LOG_UBLOX_SVINFO 'I'
#define LOG_UBLOX_POSLLH 'P'
#define LOG_UBLOX_VELNED 'V'
char gpsPacketLog[16];
static char *gpsPacketLogChar = gpsPacketLog;
// **********************
// GPS
// **********************
@ -78,7 +89,7 @@ static gpsConfig_t *gpsConfig;
#define GPS_TIMEOUT (2500)
// How many entries in gpsInitData array below
#define GPS_INIT_ENTRIES (GPS_BAUDRATE_MAX + 1)
#define GPS_BAUDRATE_CHATE_DELAY (100)
#define GPS_BAUDRATE_CHANGE_DELAY (200)
static serialConfig_t *serialConfig;
static serialPort_t *gpsPort;
@ -97,7 +108,7 @@ static const gpsInitData_t gpsInitData[] = {
{ GPS_BAUDRATE_38400, 38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
{ GPS_BAUDRATE_19200, 19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
// 9600 is not enough for 5Hz updates - leave for compatibility to dumb NMEA that only runs at this speed
{ GPS_BAUDRATE_9600, 9600, "", "" }
{ GPS_BAUDRATE_9600, 9600, "$PUBX,41,1,0003,0001,9600,0*16\r\n", "" }
};
#define GPS_INIT_DATA_ENTRY_COUNT (sizeof(gpsInitData) / sizeof(gpsInitData[0]))
@ -111,11 +122,14 @@ static const uint8_t ubloxInit[] = {
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F, // GGA: Global positioning system fix data
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x02, 0x00, 0xFC, 0x13, // GSA: GNSS DOP and Active Satellites
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x00, 0xFE, 0x17, // RMC: Recommended Minimum data
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47, // set POSLLH MSG rate
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x03, 0x01, 0x0F, 0x49, // set STATUS MSG rate
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x06, 0x01, 0x12, 0x4F, // set SOL MSG rate
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x30, 0x01, 0x3C, 0xA3, // set SVINFO MSG rate
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67, // set VELNED MSG rate
0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A, // set rate to 5Hz
0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A, // set rate to 5Hz (measurement period: 200ms, navigation rate: 1 cycle)
};
// UBlox 6 Protocol documentation - GPS.G6-SW-10018-F
@ -182,6 +196,8 @@ void gpsInit(serialConfig_t *initialSerialConfig, gpsConfig_t *initialGpsConfig)
// clear error counter
gpsData.errors = 0;
memset(gpsPacketLog, 0x00, sizeof(gpsPacketLog));
gpsConfig = initialGpsConfig;
// init gpsData structure. if we're not actually enabled, don't bother doing anything else
@ -230,17 +246,25 @@ void gpsInitUblox(void)
switch (gpsData.state) {
case GPS_INITIALIZING:
now = millis();
if (now - gpsData.state_ts < GPS_BAUDRATE_CHATE_DELAY)
if (now - gpsData.state_ts < GPS_BAUDRATE_CHANGE_DELAY)
return;
if (gpsData.state_position < GPS_INIT_ENTRIES) {
// try different speed to INIT
serialSetBaudRate(gpsPort, gpsInitData[gpsData.state_position].baudrate);
// but print our FIXED init string for the baudrate we want to be at
uint32_t newBaudRate = gpsInitData[gpsData.state_position].baudrate;
gpsData.state_ts = now;
if (serialGetBaudRate(gpsPort) != newBaudRate) {
// change the rate if needed and wait a little
serialSetBaudRate(gpsPort, newBaudRate);
return;
}
// print our FIXED init string for the baudrate we want to be at
serialPrint(gpsPort, gpsInitData[gpsData.baudrateIndex].ubx);
gpsData.state_position++;
gpsData.state_ts = now;
} else {
// we're now (hopefully) at the correct rate, next state will switch to it
gpsSetState(GPS_CHANGE_BAUD);
@ -251,6 +275,13 @@ void gpsInitUblox(void)
gpsSetState(GPS_CONFIGURE);
break;
case GPS_CONFIGURE:
// Either use specific config file for GPS or let dynamically upload config
if( gpsConfig->autoConfig == GPS_AUTOCONFIG_OFF ) {
gpsSetState(GPS_RECEIVING_DATA);
break;
}
if (gpsData.messageState == GPS_MESSAGE_STATE_IDLE) {
gpsData.messageState++;
}
@ -258,10 +289,7 @@ void gpsInitUblox(void)
if (gpsData.messageState == GPS_MESSAGE_STATE_INIT) {
if (gpsData.state_position < sizeof(ubloxInit)) {
//Either use specific config file for GPS or let dynamically upload config
if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) {
serialWrite(gpsPort, ubloxInit[gpsData.state_position]);
}
serialWrite(gpsPort, ubloxInit[gpsData.state_position]);
gpsData.state_position++;
} else {
gpsData.state_position = 0;
@ -271,10 +299,7 @@ void gpsInitUblox(void)
if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) {
if (gpsData.state_position < UBLOX_SBAS_MESSAGE_LENGTH) {
//Either use specific config file for GPS or let dynamically upload config
if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) {
serialWrite(gpsPort, ubloxSbas[gpsConfig->sbasMode].message[gpsData.state_position]);
}
serialWrite(gpsPort, ubloxSbas[gpsConfig->sbasMode].message[gpsData.state_position]);
gpsData.state_position++;
} else {
gpsData.messageState++;
@ -282,8 +307,8 @@ void gpsInitUblox(void)
}
if (gpsData.messageState >= GPS_MESSAGE_STATE_ENTRY_COUNT) {
// ublox should be initialised, try receiving
gpsSetState(GPS_RECEIVING_DATA);
// ublox should be init'd, time to try receiving
}
break;
}
@ -322,9 +347,11 @@ void gpsThread(void)
case GPS_LOST_COMMUNICATION:
gpsData.errors++;
// try another rate
gpsData.baudrateIndex++;
gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
if (gpsConfig->autoBaud) {
// try another rate
gpsData.baudrateIndex++;
gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
}
gpsData.lastMessage = millis();
// TODO - move some / all of these into gpsData
GPS_numSat = 0;
@ -554,8 +581,11 @@ static bool gpsNewFrameNMEA(char c)
if (checksum_param) { //parity checksum
uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
if (checksum == parity) {
*gpsPacketLogChar = '?';
switch (gps_frame) {
case FRAME_GGA:
*gpsPacketLogChar = LOG_NMEA_GGA;
frameOK = 1;
if (STATE(GPS_FIX)) {
GPS_coord[LAT] = gps_Msg.latitude;
@ -565,6 +595,7 @@ static bool gpsNewFrameNMEA(char c)
}
break;
case FRAME_RMC:
*gpsPacketLogChar = LOG_NMEA_RMC;
GPS_speed = gps_Msg.speed;
GPS_ground_course = gps_Msg.ground_course;
break;
@ -735,9 +766,16 @@ void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
static bool UBLOX_parse_gps(void)
{
int i;
uint32_t i;
for (i = sizeof(gpsPacketLog) - 1; i > 0 ; i--) {
gpsPacketLog[i] = gpsPacketLog[i-1];
}
*gpsPacketLogChar = '?';
switch (_msg_id) {
case MSG_POSLLH:
*gpsPacketLogChar = LOG_UBLOX_POSLLH;
//i2c_dataset.time = _buffer.posllh.time;
GPS_coord[LON] = _buffer.posllh.longitude;
GPS_coord[LAT] = _buffer.posllh.latitude;
@ -750,11 +788,13 @@ static bool UBLOX_parse_gps(void)
_new_position = true;
break;
case MSG_STATUS:
*gpsPacketLogChar = LOG_UBLOX_STATUS;
next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
if (!next_fix)
DISABLE_STATE(GPS_FIX);
break;
case MSG_SOL:
*gpsPacketLogChar = LOG_UBLOX_SOL;
next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
if (!next_fix)
DISABLE_STATE(GPS_FIX);
@ -762,12 +802,14 @@ static bool UBLOX_parse_gps(void)
GPS_hdop = _buffer.solution.position_DOP;
break;
case MSG_VELNED:
*gpsPacketLogChar = LOG_UBLOX_VELNED;
// speed_3d = _buffer.velned.speed_3d; // cm/s
GPS_speed = _buffer.velned.speed_2d; // cm/s
GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
_new_speed = true;
break;
case MSG_SVINFO:
*gpsPacketLogChar = LOG_UBLOX_SVINFO;
GPS_numCh = _buffer.svinfo.numCh;
if (GPS_numCh > 16)
GPS_numCh = 16;
@ -796,32 +838,32 @@ static bool gpsNewFrameUBLOX(uint8_t data)
bool parsed = false;
switch (_step) {
case 1:
case 1: // Sync char 2 (0x62)
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
case 0:
case 0: // Sync char 1 (0xB5)
if (PREAMBLE1 == data)
_step++;
break;
case 2:
case 2: // Class
_step++;
_class = data;
_ck_b = _ck_a = data; // reset the checksum accumulators
break;
case 3:
case 3: // Id
_step++;
_ck_b += (_ck_a += data); // checksum byte
_msg_id = data;
break;
case 4:
case 4: // Payload length (part 1)
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte
break;
case 5:
case 5: // Payload length (part 2)
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length += (uint16_t)(data << 8);
@ -870,6 +912,7 @@ gpsEnablePassthroughResult_e gpsEnablePassthrough(void)
return GPS_PASSTHROUGH_NO_SERIAL_PORT;
}
}
serialSetBaudRate(gpsPort, serialConfig->gps_baudrate);
if(!(gpsPort->mode & MODE_TX))
serialSetMode(gpsPort, gpsPort->mode | MODE_TX);

14
src/main/io/gps.h
View File

@ -48,15 +48,22 @@ typedef enum {
} gpsBaudRate_e;
typedef enum {
GPS_AUTOCONFIG_ON = 0,
GPS_AUTOCONFIG_OFF
GPS_AUTOCONFIG_OFF = 0,
GPS_AUTOCONFIG_ON,
} gpsAutoConfig_e;
typedef enum {
GPS_AUTOBAUD_OFF = 0,
GPS_AUTOBAUD_ON
} gpsAutoBaud_e;
#define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600
typedef struct gpsConfig_s {
gpsProvider_e provider;
sbasMode_e sbasMode;
gpsAutoConfig_e gpsAutoConfig;
gpsAutoConfig_e autoConfig;
gpsAutoBaud_e autoBaud;
} gpsConfig_t;
typedef enum {
@ -95,6 +102,7 @@ typedef struct gpsData_t {
extern gpsData_t gpsData;
extern int32_t GPS_coord[2]; // LAT/LON
extern char gpsPacketLog[16];
extern uint8_t GPS_numSat;
extern uint16_t GPS_hdop; // GPS signal quality
extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update

3
src/main/io/serial_cli.c
View File

@ -250,7 +250,8 @@ const clivalue_t valueTable[] = {
{ "gps_provider", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsConfig.provider, 0, GPS_PROVIDER_MAX },
{ "gps_sbas_mode", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsConfig.sbasMode, 0, SBAS_MODE_MAX },
{ "gps_auto_config", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsConfig.gpsAutoConfig, 0, GPS_AUTOCONFIG_OFF },
{ "gps_auto_config", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsConfig.autoConfig, GPS_AUTOCONFIG_OFF, GPS_AUTOCONFIG_ON },
{ "gps_auto_baud", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsConfig.autoBaud, GPS_AUTOBAUD_OFF, GPS_AUTOBAUD_ON },
{ "gps_pos_p", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDPOS], 0, 200 },