Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00
Code Issues Wiki Activity

Beginning of the great GPS unfucking.

Browse source 
* Proper initialization sequence framework for various supported GPS types. NMEA will now auto-detect its baud rate based on received frames.
* As a result of the above, gps_baudrate has been changed to enum, to only allow fixed rates. (GPS baudrate, -1: autodetect (NMEA only), 0: 115200, 1: 57600, 2: 38400, 3: 19200, 4: 9600)
* UBX binary initialization at any specified baudrate with auto-reconnect on signal loss.
* GPS thread to handle initialization, signal loss and configuration. No longer does GPS need to be powered before FC, and on GPS reconnect, it will be re-initialized if needed.
MTK NMEA/binary initialization is omitted for now, as I can't find my MTK GPS
GPS deltaTime can be calculated to display update rate.


git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@438 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2013-10-13 15:25:45 +00:00
parent 04560808e7
commit 30ded7ff04
8 changed files with 209 additions and 126 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/board.h
View file

@ -86,7 +86,8 @@ typedef enum {
typedef enum { typedef enum {
GPS_NMEA = 0, GPS_NMEA = 0,
GPS_UBLOX, GPS_UBLOX,
GPS_MTK, GPS_MTK_NMEA,
GPS_MTK_BINARY,
} GPSHardware; } GPSHardware;
typedef enum { typedef enum {

4
src/cli.c
View file

@ -117,7 +117,8 @@ const clivalue_t valueTable[] = {
{ "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 }, { "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 },
{ "softserial_baudrate", VAR_UINT32, &mcfg.softserial_baudrate, 9600, 19200 }, { "softserial_baudrate", VAR_UINT32, &mcfg.softserial_baudrate, 9600, 19200 },
{ "softserial_inverted", VAR_UINT8, &mcfg.softserial_inverted, 0, 1 }, { "softserial_inverted", VAR_UINT8, &mcfg.softserial_inverted, 0, 1 },
{ "gps_baudrate", VAR_UINT32, &mcfg.gps_baudrate, 1200, 115200 }, { "gps_type", VAR_UINT8, &mcfg.gps_type, 0, 3 },
{ "gps_baudrate", VAR_INT8, &mcfg.gps_baudrate, -1, 4 },
{ "serialrx_type", VAR_UINT8, &mcfg.serialrx_type, 0, 2 }, { "serialrx_type", VAR_UINT8, &mcfg.serialrx_type, 0, 2 },
{ "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 }, { "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 },
{ "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 }, { "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 },
@ -132,7 +133,6 @@ const clivalue_t valueTable[] = {
{ "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 }, { "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 },
{ "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 }, { "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 },
{ "gyro_cmpfm_factor", VAR_UINT16, &mcfg.gyro_cmpfm_factor, 100, 1000 }, { "gyro_cmpfm_factor", VAR_UINT16, &mcfg.gyro_cmpfm_factor, 100, 1000 },
{ "gps_type", VAR_UINT8, &mcfg.gps_type, 0, 3 },
{ "pid_controller", VAR_UINT8, &cfg.pidController, 0, 1 }, { "pid_controller", VAR_UINT8, &cfg.pidController, 0, 1 },
{ "deadband", VAR_UINT8, &cfg.deadband, 0, 32 }, { "deadband", VAR_UINT8, &cfg.deadband, 0, 32 },
{ "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 }, { "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 },

6
src/config.c
View file

@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles
config_t cfg; // profile config struct config_t cfg; // profile config struct
const char rcChannelLetters[] = "AERT1234"; const char rcChannelLetters[] = "AERT1234";
static const uint8_t EEPROM_CONF_VERSION = 52; static const uint8_t EEPROM_CONF_VERSION = 53;
static uint32_t enabledSensors = 0; static uint32_t enabledSensors = 0;
static void resetConf(void); static void resetConf(void);
@ -84,7 +84,7 @@ void readEEPROM(void)
} }
setPIDController(cfg.pidController); setPIDController(cfg.pidController);
GPS_set_pids(); gpsSetPIDs();
} }
void writeEEPROM(uint8_t b, uint8_t updateProfile) void writeEEPROM(uint8_t b, uint8_t updateProfile)
@ -205,7 +205,7 @@ static void resetConf(void)
mcfg.servo_pwm_rate = 50; mcfg.servo_pwm_rate = 50;
// gps/nav stuff // gps/nav stuff
mcfg.gps_type = GPS_NMEA; mcfg.gps_type = GPS_NMEA;
mcfg.gps_baudrate = 115200; mcfg.gps_baudrate = 0;
// serial (USART1) baudrate // serial (USART1) baudrate
mcfg.serial_baudrate = 115200; mcfg.serial_baudrate = 115200;
mcfg.softserial_baudrate = 19200; mcfg.softserial_baudrate = 19200;

270
src/gps.c
View file

@ -5,105 +5,197 @@
#define sq(x) ((x)*(x)) #define sq(x) ((x)*(x))
#endif #endif
const uint32_t init_speed[5] = { 9600, 19200, 38400, 57600, 115200 }; // GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second)
#define GPS_TIMEOUT (2500)
// How many entries in gpsInitData array below
#define GPS_INIT_ENTRIES (5)
static void GPS_NewData(uint16_t c); typedef struct gpsInitData_t {
static void gpsPrint(const char *str); uint32_t baudrate;
const char *ubx;
const char *mtk;
} gpsInitData_t;
#define UBX_INIT_STRING_INDEX 0 static const gpsInitData_t gpsInitData[] = {
#define MTK_INIT_STRING_INDEX 4 { 115200, "$PUBX,41,1,0003,0001,115200,0*1E\r\n", "$PMTK251,115200*1F\r\n" },
{ 57600, "$PUBX,41,1,0003,0001,57600,0*2D\r\n", "$PMTK251,57600*2C\r\n" },
static const char * const gpsInitStrings[] = { { 38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
"$PUBX,41,1,0003,0001,19200,0*23\r\n", // UBX0..3 { 19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
"$PUBX,41,1,0003,0001,38400,0*26\r\n", // 9600 is not enough for 5Hz updates - leave for compatibility to dumb NMEA that only runs at this speed
"$PUBX,41,1,0003,0001,57600,0*2D\r\n", { 9600, "", "" }
"$PUBX,41,1,0003,0001,115200,0*1E\r\n",
"$PMTK251,19200*22\r\n", // MTK4..7
"$PMTK251,38400*27\r\n",
"$PMTK251,57600*2C\r\n",
"$PMTK251,115200*1F\r\n",
}; };
static const uint8_t ubloxInit[] = { static const uint8_t ubloxInit[] = {
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19, // disable all default NMEA messages 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19, // VGS: Course over ground and Ground speed
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15, 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15, // GSV: GNSS Satellites in View
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11, 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11, // GLL: Latitude and longitude, with time of position fix and status
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F, 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, 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, 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, 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, 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, 0x06, 0x01, 0x12, 0x4F, // set SOL MSG rate
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67, // set VELNED MSG rate 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67, // set VELNED MSG rate
0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x51, 0x08, 0x00, 0x00, 0x8A, 0x41, // set WAAS to EGNOS 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x51, 0x08, 0x00, 0x00, 0x8A, 0x41, // set WAAS to EGNOS
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
}; };
void gpsInit(uint32_t baudrate) static const char *mtkNMEAInit[] = {
"$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n", // only GGA and RMC sentence
"$PMTK220,200*2C\r\n" // 5 Hz update rate
};
static const char *mtkBinaryInit[] = {
"$PMTK319,1*24\r\n", // SBAS Integrity Mode
"$PMTK220,200*2C\r\n", // 5 Hz update rate
"$PMTK397,0*23\r\n", // NAVTHRES_OFF
"$PMTK313,1*2E\r\n", // SBAS_ON
"$PMTK301,2*2E\r\n", // WAAS_ON
"$PGCMD,16,0,0,0,0,0*6A\r\n" // Binary ON
};
enum {
GPS_UNKNOWN,
GPS_INITIALIZING,
GPS_INITDONE,
GPS_RECEIVINGDATA,
GPS_LOSTCOMMS,
};
typedef struct gpsData_t {
uint8_t state; // GPS thread state. Used for detecting cable disconnects and configuring attached devices
uint8_t baudrateIndex; // index into auto-detecting or current baudrate
int errors; // gps error counter - crc error/lost of data/sync etc. reset on each reinit.
uint32_t lastMessage; // last time valid GPS data was received (millis)
uint32_t lastLastMessage; // last-last valid GPS message. Used to calculate delta.
} gpsData_t;
gpsData_t gpsData;
static void gpsNewData(uint16_t c);
static bool gpsNewFrameNMEA(char c);
static bool gpsNewFrameUBLOX(uint8_t data);
void gpsInit(uint8_t baudrate)
{
portMode_t mode = MODE_RXTX;
// init gpsData structure. if we're not actually enabled, don't bother doing anything else
gpsData.state = GPS_UNKNOWN;
if (!feature(FEATURE_GPS))
return;
gpsData.baudrateIndex = baudrate;
gpsData.lastMessage = millis();
gpsData.errors = 0;
// only RX is needed for NMEA-style GPS
if (mcfg.gps_type == GPS_NMEA)
mode = MODE_RX;
gpsSetPIDs();
core.gpsport = uartOpen(USART2, gpsNewData, gpsInitData[baudrate].baudrate, mode);
// signal GPS "thread" to initialize when it gets to it
gpsData.state = GPS_INITIALIZING;
}
void gpsInitHardware(void)
{ {
int i; int i;
int offset = 0;
GPS_set_pids(); switch (mcfg.gps_type) {
core.gpsport = uartOpen(USART2, GPS_NewData, baudrate, MODE_RXTX); case GPS_NMEA:
// nothing to do, just set baud rate and try receiving some stuff and see if it parses
serialSetBaudRate(core.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
gpsData.state = GPS_RECEIVINGDATA;
return;
if (mcfg.gps_type == GPS_UBLOX) case GPS_UBLOX:
offset = UBX_INIT_STRING_INDEX; // UBX will run at mcfg.baudrate, it shouldn't be "autodetected". So here we force it to that rate
else if (mcfg.gps_type == GPS_MTK) if (gpsData.state == GPS_INITIALIZING) {
offset = MTK_INIT_STRING_INDEX; for (i = 0; i < GPS_INIT_ENTRIES; i++) {
// try different speed to INIT
if (mcfg.gps_type != GPS_NMEA) { serialSetBaudRate(core.gpsport, gpsInitData[i].baudrate);
for (i = 0; i < 5; i++) { // but print our FIXED init string for the baudrate we want to be at
serialSetBaudRate(core.gpsport, init_speed[i]); serialPrint(core.gpsport, gpsInitData[mcfg.gps_baudrate].ubx);
// verify the requested change took effect. delay(200);
baudrate = serialGetBaudRate(core.gpsport);
switch (baudrate) {
case 19200:
gpsPrint(gpsInitStrings[offset]);
break;
case 38400:
gpsPrint(gpsInitStrings[offset + 1]);
break;
case 57600:
gpsPrint(gpsInitStrings[offset + 2]);
break;
case 115200:
gpsPrint(gpsInitStrings[offset + 3]);
break;
} }
delay(10); // we're now (hopefully) at the correct rate, next state will switch to it
} gpsData.baudrateIndex = mcfg.gps_baudrate;
} gpsData.state = GPS_INITDONE;
} else {
serialSetBaudRate(core.gpsport, baudrate); // GPS_INITDONE, set our real baud rate and push some ublox config strings
if (mcfg.gps_type == GPS_UBLOX) { serialSetBaudRate(core.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
for (i = 0; i < sizeof(ubloxInit); i++) { for (i = 0; i < sizeof(ubloxInit); i++) {
serialWrite(core.gpsport, ubloxInit[i]); // send ubx init binary serialWrite(core.gpsport, ubloxInit[i]); // send ubx init binary
delay(4); delay(4);
} }
} else if (mcfg.gps_type == GPS_MTK) { // ublox should be init'd, time to try receiving some junk
gpsPrint("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n"); // only GGA and RMC sentence gpsData.state = GPS_RECEIVINGDATA;
gpsPrint("$PMTK220,200*2C\r\n"); // 5 Hz update rate }
break;
case GPS_MTK_NMEA:
case GPS_MTK_BINARY:
// TODO. need to find my old piece of shit MTK GPS.
break;
} }
// catch some GPS frames. TODO check this // clear error counter
delay(1000); gpsData.errors = 0;
if (GPS_Present)
sensorsSet(SENSOR_GPS);
} }
static void gpsPrint(const char *str) void gpsThread(void)
{ {
while (*str) { switch (gpsData.state) {
serialWrite(core.gpsport, *str); case GPS_UNKNOWN:
if (mcfg.gps_type == GPS_UBLOX) break;
delay(4);
str++; case GPS_INITIALIZING:
case GPS_INITDONE:
gpsInitHardware();
break;
case GPS_LOSTCOMMS:
gpsData.errors++;
// try another rate
gpsData.baudrateIndex++;
gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
gpsData.lastMessage = millis();
// TODO - move some / all of these into gpsData
GPS_numSat = 0;
f.GPS_FIX = 0;
gpsData.state = GPS_INITIALIZING;
break;
case GPS_RECEIVINGDATA:
// check for no data/gps timeout/cable disconnection etc
if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
// remove GPS from capability
sensorsClear(SENSOR_GPS);
gpsData.state = GPS_LOSTCOMMS;
} }
// wait to send all break;
while (!isSerialTransmitBufferEmpty(core.gpsport));
delay(30);
} }
}
static bool gpsNewFrame(uint8_t c)
{
switch (mcfg.gps_type) {
case GPS_NMEA: // NMEA
case GPS_MTK_NMEA: // MTK in NMEA mode
return gpsNewFrameNMEA(c);
case GPS_UBLOX: // UBX binary
return gpsNewFrameUBLOX(c);
case GPS_MTK_BINARY: // MTK in BINARY mode (TODO)
return false;
}
return false;
}
/*----------------------------------------------------------- /*-----------------------------------------------------------
* *
@ -130,9 +222,6 @@ static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng,
static void GPS_calc_poshold(void); static void GPS_calc_poshold(void);
static void GPS_calc_nav_rate(int max_speed); static void GPS_calc_nav_rate(int max_speed);
static void GPS_update_crosstrack(void); static void GPS_update_crosstrack(void);
static bool GPS_newFrame(char c);
static bool GPS_NMEA_newFrame(char c);
static bool GPS_UBLOX_newFrame(uint8_t data);
static bool UBLOX_parse_gps(void); static bool UBLOX_parse_gps(void);
static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow); static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow);
int32_t wrap_18000(int32_t error); int32_t wrap_18000(int32_t error);
@ -285,7 +374,7 @@ static int32_t nav_bearing;
// saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home // saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home
static int16_t nav_takeoff_bearing; static int16_t nav_takeoff_bearing;
void GPS_NewData(uint16_t c) static void gpsNewData(uint16_t c)
{ {
int axis; int axis;
static uint32_t nav_loopTimer; static uint32_t nav_loopTimer;
@ -293,7 +382,11 @@ void GPS_NewData(uint16_t c)
int32_t dir; int32_t dir;
int16_t speed; int16_t speed;
if (GPS_newFrame(c)) { if (gpsNewFrame(c)) {
// new data received and parsed, we're in business
gpsData.lastLastMessage = gpsData.lastMessage;
gpsData.lastMessage = millis();
sensorsSet(SENSOR_GPS);
if (GPS_update == 1) if (GPS_update == 1)
GPS_update = 0; GPS_update = 0;
else else
@ -411,7 +504,7 @@ void GPS_reset_nav(void)
} }
// Get the relevant P I D values and set the PID controllers // Get the relevant P I D values and set the PID controllers
void GPS_set_pids(void) void gpsSetPIDs(void)
{ {
posholdPID_PARAM.kP = (float)cfg.P8[PIDPOS] / 100.0f; posholdPID_PARAM.kP = (float)cfg.P8[PIDPOS] / 100.0f;
posholdPID_PARAM.kI = (float)cfg.I8[PIDPOS] / 100.0f; posholdPID_PARAM.kI = (float)cfg.I8[PIDPOS] / 100.0f;
@ -777,19 +870,6 @@ static uint8_t hex_c(uint8_t n)
return n; return n;
} }
static bool GPS_newFrame(char c)
{
switch (mcfg.gps_type) {
case GPS_NMEA: // NMEA
case GPS_MTK: // MTK outputs NMEA too
return GPS_NMEA_newFrame(c);
case GPS_UBLOX: // UBX
return GPS_UBLOX_newFrame(c);
}
return false;
}
/* This is a light implementation of a GPS frame decoding /* This is a light implementation of a GPS frame decoding
This should work with most of modern GPS devices configured to output NMEA frames. This should work with most of modern GPS devices configured to output NMEA frames.
It assumes there are some NMEA GGA frames to decode on the serial bus It assumes there are some NMEA GGA frames to decode on the serial bus
@ -805,7 +885,7 @@ static bool GPS_newFrame(char c)
#define FRAME_GGA 1 #define FRAME_GGA 1
#define FRAME_RMC 2 #define FRAME_RMC 2
static bool GPS_NMEA_newFrame(char c) static bool gpsNewFrameNMEA(char c)
{ {
uint8_t frameOK = 0; uint8_t frameOK = 0;
static uint8_t param = 0, offset = 0, parity = 0; static uint8_t param = 0, offset = 0, parity = 0;
@ -868,8 +948,6 @@ static bool GPS_NMEA_newFrame(char c)
if (!checksum_param) if (!checksum_param)
parity ^= c; parity ^= c;
} }
if (frame)
GPS_Present = 1;
return frameOK && (frame == FRAME_GGA); return frameOK && (frame == FRAME_GGA);
} }
@ -1026,7 +1104,7 @@ void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
} }
} }
static bool GPS_UBLOX_newFrame(uint8_t data) static bool gpsNewFrameUBLOX(uint8_t data)
{ {
bool parsed = false; bool parsed = false;
@ -1083,10 +1161,8 @@ static bool GPS_UBLOX_newFrame(uint8_t data)
_step = 0; _step = 0;
if (_ck_b != data) if (_ck_b != data)
break; // bad checksum break; // bad checksum
GPS_Present = 1; if (UBLOX_parse_gps())
if (UBLOX_parse_gps()) {
parsed = true; parsed = true;
}
} //end switch } //end switch
return parsed; return parsed;
} }

8
src/main.c
View file

@ -99,10 +99,12 @@ int main(void)
sbusInit(&rcReadRawFunc); sbusInit(&rcReadRawFunc);
break; break;
} }
} else { } else { // spektrum and GPS are mutually exclusive
// spektrum and GPS are mutually exclusive
// Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2. // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
if (feature(FEATURE_GPS)) // gpsInit will return if FEATURE_GPS is not enabled.
// Sanity check below - protocols other than NMEA do not support baud rate autodetection
if (mcfg.gps_type > 0 && mcfg.gps_baudrate < 0)
mcfg.gps_baudrate = 0;
gpsInit(mcfg.gps_baudrate); gpsInit(mcfg.gps_baudrate);
} }
#ifdef SONAR #ifdef SONAR

9
src/mw.c
View file

@ -45,8 +45,6 @@ uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction
uint16_t GPS_ground_course = 0; // degrees * 10 uint16_t GPS_ground_course = 0; // degrees * 10
uint8_t GPS_Present = 0; // Checksum from Gps serial
uint8_t GPS_Enable = 0;
int16_t nav[2]; int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
@ -780,6 +778,13 @@ void loop(void)
break; break;
#endif #endif
case 3: case 3:
// if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
// hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
// change this based on available hardware
if (feature(FEATURE_GPS))
gpsThread();
break;
case 4:
taskOrder++; taskOrder++;
#ifdef SONAR #ifdef SONAR
if (sensors(SENSOR_SONAR)) { if (sensors(SENSOR_SONAR)) {

11
src/mw.h
View file

@ -262,8 +262,8 @@ typedef struct master_t {
uint8_t rssi_aux_channel; // Read rssi from channel. 1+ = AUX1+, 0 to disable. uint8_t rssi_aux_channel; // Read rssi from channel. 1+ = AUX1+, 0 to disable.
// gps-related stuff // gps-related stuff
uint8_t gps_type; // Type of GPS hardware. 0: NMEA 1: UBX 2+ ?? uint8_t gps_type; // Type of GPS hardware. 0: NMEA 1: UBX 2: MTK
uint32_t gps_baudrate; // GPS baudrate int8_t gps_baudrate; // GPS baudrate, -1: autodetect (NMEA only), 0: 115200, 1: 57600, 2: 38400, 3: 19200, 4: 9600
uint32_t serial_baudrate; uint32_t serial_baudrate;
@ -362,8 +362,6 @@ extern uint16_t GPS_altitude,GPS_speed; // altitude in 0.1m
extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update
extern int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction extern int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
extern uint16_t GPS_ground_course; // degrees*10 extern uint16_t GPS_ground_course; // degrees*10
extern uint8_t GPS_Present; // Checksum from Gps serial
extern uint8_t GPS_Enable;
extern int16_t nav[2]; extern int16_t nav[2];
extern int8_t nav_mode; // Navigation mode extern int8_t nav_mode; // Navigation mode
extern int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother extern int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
@ -447,10 +445,11 @@ void systemBeep(bool onoff);
void cliProcess(void); void cliProcess(void);
// gps // gps
void gpsInit(uint32_t baudrate); void gpsInit(uint8_t baudrate);
void gpsThread(void);
void gpsSetPIDs(void);
void GPS_reset_home_position(void); void GPS_reset_home_position(void);
void GPS_reset_nav(void); void GPS_reset_nav(void);
void GPS_set_pids(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon); void GPS_set_next_wp(int32_t* lat, int32_t* lon);
int32_t wrap_18000(int32_t error); int32_t wrap_18000(int32_t error);

2
src/serial.c
View file

@ -299,7 +299,7 @@ void serialInit(uint32_t baudrate)
} }
if (feature(FEATURE_SERVO_TILT)) if (feature(FEATURE_SERVO_TILT))
availableBoxes[idx++] = BOXCAMSTAB; availableBoxes[idx++] = BOXCAMSTAB;
if (feature(FEATURE_GPS) && sensors(SENSOR_GPS)) { if (feature(FEATURE_GPS)) {
availableBoxes[idx++] = BOXGPSHOME; availableBoxes[idx++] = BOXGPSHOME;
availableBoxes[idx++] = BOXGPSHOLD; availableBoxes[idx++] = BOXGPSHOLD;
} }