Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 16:55:29 +03:00
Code Issues Wiki Activity

GPS: Protocol-independent stack. UBLOX binary driver implementation. Autoconfig works. Autobaud works.

Browse source 
GPS: Native MAG support (for future NAZA GPS support), bugfixes

GPS: Supported GPS protocols on a per-target basis
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-12-26 21:48:45 +10:00
parent a5628cc4a8
commit 69cfbb04d2
38 changed files with 1491 additions and 1249 deletions
Download patch file Download diff file Expand all files Collapse all files

3
Makefile
View file

@ -275,6 +275,9 @@ HIGHEND_SRC = \
flight/gps_conversion.c \
common/colorconversion.c \
io/gps.c \
io/gps_ublox.c \
io/gps_nmea.c \
io/gps_i2cnav.c \
io/ledstrip.c \
io/display.c \
telemetry/telemetry.c \

22
src/main/blackbox/blackbox.c
View file

@ -968,16 +968,16 @@ static void writeGPSFrame()
blackboxWriteUnsignedVB(currentTime - blackboxHistory[1]->time);
}
blackboxWriteUnsignedVB(GPS_numSat);
blackboxWriteSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]);
blackboxWriteSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]);
blackboxWriteUnsignedVB(GPS_altitude);
blackboxWriteUnsignedVB(GPS_speed);
blackboxWriteUnsignedVB(GPS_ground_course);
blackboxWriteUnsignedVB(gpsSol.numSat);
blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[0]);
blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[1]);
blackboxWriteUnsignedVB(gpsSol.llh.alt);
blackboxWriteUnsignedVB(gpsSol.groundSpeed);
blackboxWriteUnsignedVB(gpsSol.groundCourse);
gpsHistory.GPS_numSat = GPS_numSat;
gpsHistory.GPS_coord[0] = GPS_coord[0];
gpsHistory.GPS_coord[1] = GPS_coord[1];
gpsHistory.GPS_numSat = gpsSol.numSat;
gpsHistory.GPS_coord[0] = gpsSol.llh.lat;
gpsHistory.GPS_coord[1] = gpsSol.llh.lon;
}
#endif
@ -1371,8 +1371,8 @@ static void blackboxLogIteration()
writeGPSHomeFrame();
writeGPSFrame();
} else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0]
|| GPS_coord[1] != gpsHistory.GPS_coord[1]) {
} else if (gpsSol.numSat != gpsHistory.GPS_numSat || gpsSol.llh.lat != gpsHistory.GPS_coord[0]
|| gpsSol.llh.lon != gpsHistory.GPS_coord[1]) {
//We could check for velocity changes as well but I doubt it changes independent of position
writeGPSFrame();
}

12
src/main/flight/imu.c
View file

@ -461,15 +461,15 @@ static void imuCalculateEstimatedAttitude(float dT)
useMag = true;
}
#if defined(GPS)
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5 && GPS_speed >= 300) {
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
// In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
if (gpsHeadingInitialized) {
courseOverGround = DECIDEGREES_TO_RADIANS(GPS_ground_course);
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true;
}
else {
// Re-initialize quaternion from known Roll, Pitch and GPS heading
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, GPS_ground_course);
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
gpsHeadingInitialized = true;
}
}
@ -514,9 +514,9 @@ static void imuUpdateMeasuredAcceleration(float dT)
* assumption: tangential velocity only along body x-axis
* assumption: GPS velocity equal to body x-axis velocity
*/
if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5) {
imuMeasuredGravityBF.A[Y] -= GPS_speed * imuMeasuredRotationBF.A[Z];
imuMeasuredGravityBF.A[Z] += GPS_speed * imuMeasuredRotationBF.A[Y];
if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) {
imuMeasuredGravityBF.A[Y] -= gpsSol.groundSpeed * imuMeasuredRotationBF.A[Z];
imuMeasuredGravityBF.A[Z] += gpsSol.groundSpeed * imuMeasuredRotationBF.A[Y];
}
#endif

6
src/main/flight/navigation_rewrite.c
View file

@ -1886,7 +1886,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
if (posControl.flags.hasValidAltitudeSensor) navFlags |= (1 << 0);
if (posControl.flags.hasValidSurfaceSensor) navFlags |= (1 << 1);
if (posControl.flags.hasValidPositionSensor) navFlags |= (1 << 2);
if ((STATE(GPS_FIX) && GPS_numSat >= posControl.navConfig->inav.gps_min_sats)) navFlags |= (1 << 3);
if ((STATE(GPS_FIX) && gpsSol.numSat >= posControl.navConfig->inav.gps_min_sats)) navFlags |= (1 << 3);
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
#endif
@ -2241,6 +2241,7 @@ rthState_e getStateOfForcedRTH(void)
#else // NAV
#ifdef GPS
/* Fallback if navigation is not compiled in - handle GPS home coordinates */
static float GPS_scaleLonDown;
@ -2266,7 +2267,7 @@ void onNewGPSData(int32_t newLat, int32_t newLon, int32_t newAlt, int16_t velN,
UNUSED(velDValid);
UNUSED(hdop);
if (!(sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5))
if (!(sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5))
return;
if (ARMING_FLAG(ARMED)) {
@ -2292,5 +2293,6 @@ void onNewGPSData(int32_t newLat, int32_t newLon, int32_t newAlt, int16_t velN,
GPS_scaleLonDown = cos_approx((ABS((float)newLat) / 10000000.0f) * 0.0174532925f);
}
}
#endif
#endif // NAV

8
src/main/flight/navigation_rewrite.h
View file

@ -22,17 +22,11 @@
#include "io/rc_controls.h"
#include "io/escservo.h"
#include "io/gps.h"
#include "flight/pid.h"
#include "flight/failsafe.h"
/* LLH Location in NEU axis system */
typedef struct gpsLocation_s {
int32_t lat; // Lattitude * 1e+7
int32_t lon; // Longitude * 1e+7
int32_t alt; // Altitude in centimeters (meters * 100)
} gpsLocation_t;
/* GPS Home location data */
extern gpsLocation_t GPS_home;
extern uint16_t GPS_distanceToHome; // distance to home point in meters

4
src/main/flight/navigation_rewrite_pos_estimator.c
View file

@ -244,7 +244,7 @@ void onNewGPSData(int32_t newLat, int32_t newLon, int32_t newAlt, int16_t velN,
velDValid = false;
if (sensors(SENSOR_GPS)) {
if (!(STATE(GPS_FIX) && GPS_numSat >= posControl.navConfig->inav.gps_min_sats)) {
if (!(STATE(GPS_FIX) && gpsSol.numSat >= posControl.navConfig->inav.gps_min_sats)) {
isFirstGPSUpdate = true;
return;
}
@ -264,7 +264,7 @@ void onNewGPSData(int32_t newLat, int32_t newLon, int32_t newAlt, int16_t velN,
/* Process position update if GPS origin is already set, or precision is good enough */
// FIXME: use HDOP here
if ((posControl.gpsOrigin.valid) || (GPS_numSat >= posControl.navConfig->inav.gps_min_sats)) {
if ((posControl.gpsOrigin.valid) || (gpsSol.numSat >= posControl.navConfig->inav.gps_min_sats)) {
/* Convert LLH position to local coordinates */
t_fp_vector newLocalPos;
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &newLLH, &newLocalPos, GEO_ALT_ABSOLUTE);

4
src/main/io/beeper.c
View file

@ -253,12 +253,12 @@ void beeperConfirmationBeeps(uint8_t beepCount)
void beeperGpsStatus(void)
{
// if GPS fix then beep out number of satellites
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
uint8_t i = 0;
do {
beep_multiBeeps[i++] = 5;
beep_multiBeeps[i++] = 10;
} while (i < MAX_MULTI_BEEPS && GPS_numSat > i / 2);
} while (i < MAX_MULTI_BEEPS && gpsSol.numSat > i / 2);
beep_multiBeeps[i-1] = 50; // extend last pause
beep_multiBeeps[i] = BEEPER_COMMAND_STOP;

33
src/main/io/display.c
View file

@ -225,7 +225,7 @@ void updateFailsafeStatus(void)
case FAILSAFE_RX_LOSS_DETECTED:
failsafeIndicator = 'R';
break;
#ifdef NAV
#if defined(NAV)
case FAILSAFE_RETURN_TO_HOME:
failsafeIndicator = 'H';
break;
@ -338,7 +338,7 @@ void showProfilePage(void)
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}
#define SATELLITE_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0]))
#define SATELLITE_COUNT (sizeof(gpsSol.svInfo) / sizeof(gpsSol.svInfo[0]))
#define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
#ifdef GPS
@ -346,9 +346,9 @@ void showGpsPage() {
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
static uint8_t gpsTicker = 0;
static uint32_t lastGPSSvInfoReceivedCount = 0;
if (GPS_svInfoReceivedCount != lastGPSSvInfoReceivedCount) {
lastGPSSvInfoReceivedCount = GPS_svInfoReceivedCount;
static uint8_t lastGPSHeartbeat = 0;
if (gpsSol.flags.gpsHeartbeat != lastGPSHeartbeat) {
lastGPSHeartbeat = gpsSol.flags.gpsHeartbeat;
gpsTicker++;
gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT;
}
@ -360,58 +360,53 @@ void showGpsPage() {
uint32_t index;
for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) {
uint8_t bargraphOffset = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
uint8_t bargraphOffset = ((uint16_t) gpsSol.svInfo[index].cno * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
}
char fixChar = STATE(GPS_FIX) ? 'Y' : 'N';
tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", GPS_numSat, fixChar);
tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", gpsSol.numSat, fixChar);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "La/Lo: %d/%d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER);
tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "Spd: %d", GPS_speed);
tfp_sprintf(lineBuffer, "Spd: %d", gpsSol.groundSpeed);
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "GC: %d", GPS_ground_course);
tfp_sprintf(lineBuffer, "GC: %d", gpsSol.groundCourse);
padHalfLineBuffer();
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount);
tfp_sprintf(lineBuffer, "RX: %d", gpsStats.packetCount);
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "ERRs: %d", gpsData.errors, gpsData.timeouts);
tfp_sprintf(lineBuffer, "ERRs: %d", gpsStats.errors);
padHalfLineBuffer();
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "Dt: %d", gpsData.lastMessage - gpsData.lastLastMessage);
tfp_sprintf(lineBuffer, "Dt: %d", gpsStats.lastMessageDt);
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "TOs: %d", gpsData.timeouts);
tfp_sprintf(lineBuffer, "TOs: %d", gpsStats.timeouts);
padHalfLineBuffer();
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer);
strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT);
padHalfLineBuffer();
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();

1339
src/main/io/gps.c
View file

File diff suppressed because it is too large Load diff

90
src/main/io/gps.h
View file

@ -17,6 +17,11 @@
#pragma once
#define GPS_DBHZ_MIN 0
#define GPS_DBHZ_MAX 55
#define GPS_SV_MAXSATS 16
#define LAT 0
#define LON 1
@ -25,11 +30,10 @@
typedef enum {
GPS_NMEA = 0,
GPS_UBLOX,
GPS_I2C
GPS_I2CNAV,
GPS_PROVIDER_COUNT
} gpsProvider_e;
#define GPS_PROVIDER_MAX GPS_I2C
typedef enum {
SBAS_AUTO = 0,
SBAS_EGNOS,
@ -45,7 +49,8 @@ typedef enum {
GPS_BAUDRATE_57600,
GPS_BAUDRATE_38400,
GPS_BAUDRATE_19200,
GPS_BAUDRATE_9600
GPS_BAUDRATE_9600,
GPS_BAUDRATE_COUNT
} gpsBaudRate_e;
typedef enum {
@ -72,53 +77,54 @@ typedef struct gpsCoordinateDDDMMmmmm_s {
int16_t mmmm;
} gpsCoordinateDDDMMmmmm_t;
typedef struct {
uint8_t chn; // Channel number
uint8_t svid; // Satellite ID
uint8_t quality; // Bitfield Qualtity
uint8_t cno; // Carrier to Noise Ratio (Signal Strength)
} gpsSVChannel_t;
typedef enum {
GPS_MESSAGE_STATE_IDLE = 0,
GPS_MESSAGE_STATE_INIT,
GPS_MESSAGE_STATE_SBAS,
GPS_MESSAGE_STATE_MAX = GPS_MESSAGE_STATE_SBAS
} gpsMessageState_e;
/* LLH Location in NEU axis system */
typedef struct gpsLocation_s {
int32_t lat; // Lattitude * 1e+7
int32_t lon; // Longitude * 1e+7
int32_t alt; // Altitude in centimeters (meters * 100)
} gpsLocation_t;
#define GPS_MESSAGE_STATE_ENTRY_COUNT (GPS_MESSAGE_STATE_MAX + 1)
typedef struct gpsSolutionData_s {
struct {
unsigned gpsHeartbeat : 1; // Toggle each update
unsigned fix3D : 1; // gps fix status
unsigned validVelNE : 1;
unsigned validVelD : 1;
unsigned validMag : 1;
} flags;
typedef struct gpsData_s {
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
uint32_t errors; // gps error counter - crc error/lost of data/sync etc..
uint32_t timeouts;
uint32_t lastMessage; // last time valid GPS data was received (millis)
uint32_t lastLastMessage; // last-last valid GPS message. Used to calculate delta.
uint8_t numSat;
uint32_t state_position; // incremental variable for loops
uint32_t state_ts; // timestamp for last state_position increment
gpsMessageState_e messageState;
} gpsData_t;
uint8_t numCh;
gpsSVChannel_t svInfo[GPS_SV_MAXSATS];
#define GPS_PACKET_LOG_ENTRY_COUNT 21 // To make this useful we should log as many packets as we can fit characters a single line of a OLED display.
extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
gpsLocation_t llh;
int16_t magData[3];
int16_t velNED[3];
extern gpsData_t gpsData;
extern int32_t GPS_coord[2]; // LAT/LON
int16_t groundSpeed;
int16_t groundCourse;
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
extern uint32_t GPS_packetCount;
extern uint32_t GPS_svInfoReceivedCount;
extern uint16_t GPS_altitude; // altitude in 0.1m
extern uint16_t GPS_speed; // speed in 0.1m/s
extern uint16_t GPS_ground_course; // degrees * 10
extern uint8_t GPS_numCh; // Number of channels
extern uint8_t GPS_svinfo_chn[16]; // Channel number
extern uint8_t GPS_svinfo_svid[16]; // Satellite ID
extern uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
uint16_t hdop;
uint16_t vdop;
} gpsSolutionData_t;
#define GPS_DBHZ_MIN 0
#define GPS_DBHZ_MAX 55
typedef struct {
uint16_t lastMessageDt;
uint32_t errors; // gps error counter - crc error/lost of data/sync etc..
uint32_t timeouts;
uint32_t packetCount;
} gpsStatistics_t;
extern gpsSolutionData_t gpsSol;
extern gpsStatistics_t gpsStats;
void gpsThread(void);
bool gpsNewFrameFromSerial(uint8_t c);
void updateGpsIndicator(uint32_t currentTime);

125
src/main/io/gps_i2cnav.c Executable file
View file

@ -0,0 +1,125 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "build_config.h"
#include "debug.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/utils.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "drivers/sensor.h"
#include "drivers/gps.h"
#include "drivers/gps_i2cnav.h"
#include "sensors/sensors.h"
#include "io/serial.h"
#include "io/display.h"
#include "io/gps.h"
#include "io/gps_private.h"
#include "flight/gps_conversion.h"
#include "flight/pid.h"
#include "flight/hil.h"
#include "flight/navigation_rewrite.h"
#include "config/config.h"
#include "config/runtime_config.h"
#if defined(GPS) && defined(GPS_PROTO_I2C_NAV)
#define GPS_I2C_POLL_RATE_HZ 20 // Poll I2C GPS at this rate
bool gpsDetectI2CNAV(void)
{
return i2cnavGPSModuleDetect();
}
bool gpsPollI2CNAV(void)
{
gpsDataGeneric_t gpsMsg;
// Check for poll rate timeout
if ((millis() - gpsState.lastMessageMs) < (1000 / GPS_I2C_POLL_RATE_HZ))
return false;
i2cnavGPSModuleRead(&gpsMsg);
debug[2] = gpsMsg.flags.gpsOk;
debug[3] = gpsMsg.flags.newData;
if (gpsMsg.flags.gpsOk) {
gpsSol.flags.fix3D = gpsMsg.flags.fix3D;
// sat count
gpsSol.numSat = gpsMsg.numSat;
// Other data
if (gpsMsg.flags.newData) {
if (gpsMsg.flags.fix3D) {
gpsSol.hdop = gpsMsg.hdop;
gpsSol.groundSpeed = gpsMsg.speed;
gpsSol.groundCourse = gpsMsg.ground_course;
gpsSol.llh.lat = gpsMsg.latitude;
gpsSol.llh.lon = gpsMsg.longitude;
gpsSol.llh.alt = gpsMsg.altitude;
gpsSol.flags.validVelNE = 0;
gpsSol.flags.validVelD = 0;
}
gpsStats.packetCount++;
gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
return true;
}
}
return false;
}
bool gpsHandleI2CNAV(void)
{
// Process state
switch(gpsState.state) {
default:
return false;
case GPS_CONFIGURE:
gpsSetState(GPS_RECEIVING_DATA);
return false;
case GPS_RECEIVING_DATA:
return gpsPollI2CNAV();
}
}
#endif

331
src/main/io/gps_nmea.c Executable file
View file

@ -0,0 +1,331 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "build_config.h"
#include "debug.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/utils.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "drivers/sensor.h"
#include "drivers/gps.h"
#include "drivers/gps_i2cnav.h"
#include "sensors/sensors.h"
#include "io/serial.h"
#include "io/display.h"
#include "io/gps.h"
#include "io/gps_private.h"
#include "flight/gps_conversion.h"
#include "flight/pid.h"
#include "flight/hil.h"
#include "flight/navigation_rewrite.h"
#include "config/config.h"
#include "config/runtime_config.h"
#if defined(GPS) && defined(GPS_PROTO_NMEA)
/* This is a light implementation of a GPS frame decoding
This should work with most of modern GPS devices configured to output 5 frames.
It assumes there are some NMEA GGA frames to decode on the serial bus
Now verifies checksum correctly before applying data
Here we use only the following data :
- latitude
- longitude
- GPS fix is/is not ok
- GPS num sat (4 is enough to be +/- reliable)
// added by Mis
- GPS altitude (for OSD displaying)
- GPS speed (for OSD displaying)
*/
#define NO_FRAME 0
#define FRAME_GGA 1
#define FRAME_RMC 2
#define FRAME_GSV 3
static uint32_t grab_fields(char *src, uint8_t mult)
{ // convert string to uint32
uint32_t i;
uint32_t tmp = 0;
for (i = 0; src[i] != 0; i++) {
if (src[i] == '.') {
i++;
if (mult == 0)
break;
else
src[i + mult] = 0;
}
tmp *= 10;
if (src[i] >= '0' && src[i] <= '9')
tmp += src[i] - '0';
if (i >= 15)
return 0; // out of bounds
}
return tmp;
}
typedef struct gpsDataNmea_s {
bool fix;
int32_t latitude;
int32_t longitude;
uint8_t numSat;
uint16_t altitude;
uint16_t speed;
uint16_t ground_course;
} gpsDataNmea_t;
static bool gpsNewFrameNMEA(char c)
{
static gpsDataNmea_t gps_Msg;
uint8_t frameOK = 0;
static uint8_t param = 0, offset = 0, parity = 0;
static char string[15];
static uint8_t checksum_param, gps_frame = NO_FRAME;
static uint8_t svMessageNum = 0;
uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0;
switch (c) {
case '$':
param = 0;
offset = 0;
parity = 0;
break;
case ',':
case '*':
string[offset] = 0;
if (param == 0) { //frame identification
gps_frame = NO_FRAME;
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
gps_frame = FRAME_GGA;
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
gps_frame = FRAME_RMC;
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'S' && string[4] == 'V')
gps_frame = FRAME_GSV;
}
switch (gps_frame) {
case FRAME_GGA: //************* GPGGA FRAME parsing
switch(param) {
// case 1: // Time information
// break;
case 2:
gps_Msg.latitude = GPS_coord_to_degrees(string);
break;
case 3:
if (string[0] == 'S')
gps_Msg.latitude *= -1;
break;
case 4:
gps_Msg.longitude = GPS_coord_to_degrees(string);
break;
case 5:
if (string[0] == 'W')
gps_Msg.longitude *= -1;
break;
case 6:
if (string[0] > '0') {
gps_Msg.fix = true;
} else {
gps_Msg.fix = false;
}
break;
case 7:
gps_Msg.numSat = grab_fields(string, 0);
break;
case 9:
gps_Msg.altitude = grab_fields(string, 1) * 10; // altitude in meters added by Mis (cm)
break;
}
break;
case FRAME_RMC: //************* GPRMC FRAME parsing
switch(param) {
case 7:
gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis
break;
case 8:
gps_Msg.ground_course = (grab_fields(string, 1)); // ground course deg * 10
break;
}
break;
case FRAME_GSV:
switch(param) {
/*case 1:
// Total number of messages of this type in this cycle
break; */
case 2:
// Message number
svMessageNum = grab_fields(string, 0);
break;
case 3:
// Total number of SVs visible
gpsSol.numCh = grab_fields(string, 0);
break;
}
if(param < 4)
break;
svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4
svSatNum = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number
svSatParam = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite
if(svSatNum > GPS_SV_MAXSATS)
break;
switch(svSatParam) {
case 1:
// SV PRN number
gpsSol.svInfo[svSatNum - 1].chn = svSatNum;
gpsSol.svInfo[svSatNum - 1].svid = grab_fields(string, 0);
break;
/*case 2:
// Elevation, in degrees, 90 maximum
break;
case 3:
// Azimuth, degrees from True North, 000 through 359
break; */
case 4:
// SNR, 00 through 99 dB (null when not tracking)
gpsSol.svInfo[svSatNum - 1].cno = grab_fields(string, 0);
gpsSol.svInfo[svSatNum - 1].quality = 0; // only used by ublox
break;
}
break;
}
param++;
offset = 0;
if (c == '*')
checksum_param = 1;
else
parity ^= c;
break;
case '\r':
case '\n':
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) {
gpsStats.packetCount++;
switch (gps_frame) {
case FRAME_GGA:
frameOK = 1;
if (STATE(GPS_FIX)) {
gpsSol.numSat = gps_Msg.numSat;
gpsSol.llh.lat = gps_Msg.latitude;
gpsSol.llh.lon = gps_Msg.longitude;
gpsSol.llh.alt = gps_Msg.altitude;
}
// NMEA does not report VELNED
gpsSol.flags.validVelNE = 0;
gpsSol.flags.validVelD = 0;
break;
case FRAME_RMC:
gpsSol.groundSpeed = gps_Msg.speed;
gpsSol.groundCourse = gps_Msg.ground_course;
break;
} // end switch
}
else {
gpsStats.errors++;
}
}
checksum_param = 0;
break;
default:
if (offset < 15)
string[offset++] = c;
if (!checksum_param)
parity ^= c;
}
return frameOK;
}
static bool gpsInitialize(void)
{
// NMEA is receive-only, we can only cycle thru baud rates and check if we are receiving anything
gpsState.baudrateIndex = gpsState.autoBaudrateIndex;
return false;
}
static bool gpsConfigure(void)
{
// No autoconfig, switch straight to receiving data
gpsSetState(GPS_RECEIVING_DATA);
return false;
}
static bool gpsReceiveData(void)
{
bool hasNewData = false;
if (gpsState.gpsPort) {
while (serialRxBytesWaiting(gpsState.gpsPort)) {
uint8_t newChar = serialRead(gpsState.gpsPort);
if (gpsNewFrameNMEA(newChar)) {
gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
gpsSol.flags.validVelNE = 0;
gpsSol.flags.validVelD = 0;
hasNewData = true;
}
}
}
return hasNewData;
}
bool gpsHandleNMEA(void)
{
// Receive data
bool hasNewData = gpsReceiveData();
// Process state
switch(gpsState.state) {
default:
return false;
case GPS_INITIALIZING:
return gpsInitialize();
case GPS_CONFIGURE:
return gpsConfigure();
case GPS_RECEIVING_DATA:
return hasNewData;
}
}
#endif

57
src/main/io/gps_private.h Executable file
View file

@ -0,0 +1,57 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef GPS
typedef enum {
GPS_UNKNOWN, // 0
GPS_INITIALIZING, // 1
GPS_CHANGE_BAUD, // 2
GPS_CONFIGURE, // 3
GPS_RECEIVING_DATA, // 4
GPS_LOST_COMMUNICATION, // 5
} gpsState_e;
typedef struct {
gpsConfig_t * gpsConfig;
serialConfig_t * serialConfig;
serialPort_t * gpsPort; // Serial GPS only
gpsState_e state;
gpsBaudRate_e baudrateIndex;
gpsBaudRate_e autoBaudrateIndex; // Driver internal use (for autoBaud)
uint8_t autoConfigStep; // Driver internal use (for autoConfig)
uint8_t autoConfigPosition; // Driver internal use (for autoConfig)
uint32_t lastStateSwitchMs;
uint32_t lastLastMessageMs;
uint32_t lastMessageMs;
} gpsReceiverData_t;
extern gpsReceiverData_t gpsState;
extern baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT];
extern void gpsSetState(gpsState_e state);
extern bool gpsHandleNMEA(void);
extern bool gpsHandleUBLOX(void);
extern bool gpsHandleI2CNAV(void);
extern bool gpsDetectI2CNAV(void);
#endif

512
src/main/io/gps_ublox.c Executable file
View file

@ -0,0 +1,512 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "build_config.h"
#include "debug.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/utils.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "drivers/sensor.h"
#include "drivers/gps.h"
#include "drivers/gps_i2cnav.h"
#include "sensors/sensors.h"
#include "io/serial.h"
#include "io/display.h"
#include "io/gps.h"
#include "io/gps_private.h"
#include "flight/gps_conversion.h"
#include "flight/pid.h"
#include "flight/hil.h"
#include "flight/navigation_rewrite.h"
#include "config/config.h"
#include "config/runtime_config.h"
#if defined(GPS) && defined(GPS_PROTO_UBLOX)
static const char * baudInitData[GPS_BAUDRATE_COUNT] = {
"$PUBX,41,1,0003,0001,115200,0*1E\r\n", // GPS_BAUDRATE_115200
"$PUBX,41,1,0003,0001,57600,0*2D\r\n", // GPS_BAUDRATE_57600
"$PUBX,41,1,0003,0001,38400,0*26\r\n", // GPS_BAUDRATE_38400
"$PUBX,41,1,0003,0001,19200,0*23\r\n", // GPS_BAUDRATE_19200
"$PUBX,41,1,0003,0001,9600,0*16\r\n" // GPS_BAUDRATE_9600
};
static const uint8_t ubloxInit6[] = {
0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x02, 0x00, // CFG-NAV5 - Set engine settings
0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Airborne <1G 3D fix only
0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x51, 0xC7,
0xB5, 0x62, 0x06, 0x23, 0x28, 0x00, 0x00, 0x00, 0x4C, 0x66, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // CFG-NAVX5 min 5 SV
0x05, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7C, 0xCD,
// DISABLE 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, // GSV: GNSS Satellites in View
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, // 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
// Enable UBLOX messages
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 (every cycle - high bandwidth)
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x30, 0x05, 0x40, 0xA7, // set SVINFO MSG rate (evey 5 cycles - low bandwidth)
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 (measurement period: 200ms, navigation rate: 1 cycle)
};
// UBlox 6 Protocol documentation - GPS.G6-SW-10018-F
// SBAS Configuration Settings Desciption, Page 4/210
// 31.21 CFG-SBAS (0x06 0x16), Page 142/210
// A.10 SBAS Configuration (UBX-CFG-SBAS), Page 198/210 - GPS.G6-SW-10018-F
#define UBLOX_SBAS_MESSAGE_LENGTH 16
typedef struct ubloxSbas_s {
sbasMode_e mode;
uint8_t message[UBLOX_SBAS_MESSAGE_LENGTH];
} ubloxSbas_t;
// Note: these must be defined in the same order is sbasMode_e since no lookup table is used.
static const ubloxSbas_t ubloxSbas[] = {
// NOTE this could be optimized to save a few bytes of flash space since the same prefix is used for each command.
{ SBAS_AUTO, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x31, 0xE5}},
{ SBAS_EGNOS, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x51, 0x08, 0x00, 0x00, 0x8A, 0x41}},
{ SBAS_WAAS, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x04, 0xE0, 0x04, 0x00, 0x19, 0x9D}},
{ SBAS_MSAS, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x00, 0x02, 0x02, 0x00, 0x35, 0xEF}},
{ SBAS_GAGAN, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x80, 0x01, 0x00, 0x00, 0xB2, 0xE8}}
};
// UBX support
typedef struct {
uint8_t preamble1;
uint8_t preamble2;
uint8_t msg_class;
uint8_t msg_id;
uint16_t length;
} ubx_header;
typedef struct {
uint32_t time; // GPS msToW
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
} ubx_nav_posllh;
typedef struct {
uint32_t time; // GPS msToW
uint8_t fix_type;
uint8_t fix_status;
uint8_t differential_status;
uint8_t res;
uint32_t time_to_first_fix;
uint32_t uptime; // milliseconds
} ubx_nav_status;
typedef struct {
uint32_t time;
int32_t time_nsec;
int16_t week;
uint8_t fix_type;
uint8_t fix_status;
int32_t ecef_x;
int32_t ecef_y;
int32_t ecef_z;
uint32_t position_accuracy_3d;
int32_t ecef_x_velocity;
int32_t ecef_y_velocity;
int32_t ecef_z_velocity;
uint32_t speed_accuracy;
uint16_t position_DOP;
uint8_t res;
uint8_t satellites;
uint32_t res2;
} ubx_nav_solution;
typedef struct {
uint32_t time; // GPS msToW
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
uint32_t speed_3d;
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
} ubx_nav_velned;
typedef struct {
uint8_t chn; // Channel number, 255 for SVx not assigned to channel
uint8_t svid; // Satellite ID
uint8_t flags; // Bitmask
uint8_t quality; // Bitfield
uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
uint8_t elev; // Elevation in integer degrees
int16_t azim; // Azimuth in integer degrees
int32_t prRes; // Pseudo range residual in centimetres
} ubx_nav_svinfo_channel;
typedef struct {
uint32_t time; // GPS Millisecond time of week
uint8_t numCh; // Number of channels
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
uint16_t reserved2; // Reserved
ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
} ubx_nav_svinfo;
enum {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
CLASS_NAV = 0x01,
CLASS_ACK = 0x05,
CLASS_CFG = 0x06,
MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01,
MSG_POSLLH = 0x2,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_VELNED = 0x12,
MSG_SVINFO = 0x30,
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24
} ubx_protocol_bytes;
enum {
FIX_NONE = 0,
FIX_DEAD_RECKONING = 1,
FIX_2D = 2,
FIX_3D = 3,
FIX_GPS_DEAD_RECKONING = 4,
FIX_TIME = 5
} ubs_nav_fix_type;
enum {
NAV_STATUS_FIX_VALID = 1
} ubx_nav_status_bits;
// Packet checksum accumulators
static uint8_t _ck_a;
static uint8_t _ck_b;
// State machine state
static bool _skip_packet;
static uint8_t _step;
static uint8_t _msg_id;
static uint16_t _payload_length;
static uint16_t _payload_counter;
static bool next_fix;
static uint8_t _class;
// do we have new position information?
static bool _new_position;
// do we have new speed information?
static bool _new_speed;
// Example packet sizes from UBlox u-center from a Glonass capable GPS receiver.
//15:17:55 R -> UBX NAV-STATUS, Size 24, 'Navigation Status'
//15:17:55 R -> UBX NAV-POSLLH, Size 36, 'Geodetic Position'
//15:17:55 R -> UBX NAV-VELNED, Size 44, 'Velocity in WGS 84'
//15:17:55 R -> UBX NAV-CLOCK, Size 28, 'Clock Status'
//15:17:55 R -> UBX NAV-AOPSTATUS, Size 24, 'AOP Status'
//15:17:55 R -> UBX 03-09, Size 208, 'Unknown'
//15:17:55 R -> UBX 03-10, Size 336, 'Unknown'
//15:17:55 R -> UBX NAV-SOL, Size 60, 'Navigation Solution'
//15:17:55 R -> UBX NAV, Size 100, 'Navigation'
//15:17:55 R -> UBX NAV-SVINFO, Size 328, 'Satellite Status and Information'
// from the UBlox6 document, the largest payout we receive i the NAV-SVINFO and the payload size
// is calculated as 8 + 12*numCh. numCh in the case of a Glonass receiver is 28.
#define UBLOX_PAYLOAD_SIZE 344
// Receive buffer
static union {
ubx_nav_posllh posllh;
ubx_nav_status status;
ubx_nav_solution solution;
ubx_nav_velned velned;
ubx_nav_svinfo svinfo;
uint8_t bytes[UBLOX_PAYLOAD_SIZE];
} _buffer;
void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
{
while (len--) {
*ck_a += *data;
*ck_b += *ck_a;
data++;
}
}
static bool gpsParceFrameUBLOX(void)
{
uint32_t i;
switch (_msg_id) {
case MSG_POSLLH:
//i2c_dataset.time = _buffer.posllh.time;
gpsSol.llh.lon = _buffer.posllh.longitude;
gpsSol.llh.lat = _buffer.posllh.latitude;
gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm
gpsSol.flags.fix3D = next_fix ? 1 : 0;
_new_position = true;
break;
case MSG_STATUS:
next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
if (!next_fix)
gpsSol.flags.fix3D = 0;
break;
case MSG_SOL:
next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
if (!next_fix)
gpsSol.flags.fix3D = 0;
gpsSol.numSat = _buffer.solution.satellites;
gpsSol.hdop = _buffer.solution.position_DOP;
break;
case MSG_VELNED:
gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s
gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
gpsSol.velNED[0] = _buffer.velned.ned_north;
gpsSol.velNED[1] = _buffer.velned.ned_east;
gpsSol.velNED[2] = _buffer.velned.ned_down;
gpsSol.flags.validVelNE = 1;
gpsSol.flags.validVelD = 1;
_new_speed = true;
break;
case MSG_SVINFO:
gpsSol.numCh = _buffer.svinfo.numCh;
if (gpsSol.numCh > 16)
gpsSol.numCh = 16;
for (i = 0; i < gpsSol.numCh; i++){
gpsSol.svInfo[i].chn = _buffer.svinfo.channel[i].chn;
gpsSol.svInfo[i].svid = _buffer.svinfo.channel[i].svid;
gpsSol.svInfo[i].quality = _buffer.svinfo.channel[i].quality;
gpsSol.svInfo[i].cno = _buffer.svinfo.channel[i].cno;
}
break;
default:
return false;
}
// we only return true when we get new position and speed data
// this ensures we don't use stale data
if (_new_position && _new_speed) {
gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
_new_speed = _new_position = false;
return true;
}
return false;
}
static bool gpsNewFrameUBLOX(uint8_t data)
{
bool parsed = false;
switch (_step) {
case 0: // Sync char 1 (0xB5)
if (PREAMBLE1 == data) {
_skip_packet = false;
_step++;
}
break;
case 1: // Sync char 2 (0x62)
if (PREAMBLE2 != data) {
_step = 0;
break;
}
_step++;
break;
case 2: // Class
_step++;
_class = data;
_ck_b = _ck_a = data; // reset the checksum accumulators
break;
case 3: // Id
_step++;
_ck_b += (_ck_a += data); // checksum byte
_msg_id = data;
break;
case 4: // Payload length (part 1)
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte
break;
case 5: // Payload length (part 2)
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length += (uint16_t)(data << 8);
if (_payload_length > UBLOX_PAYLOAD_SIZE) {
_skip_packet = true;
}
_payload_counter = 0; // prepare to receive payload
if (_payload_length == 0) {
_step = 7;
}
break;
case 6:
_ck_b += (_ck_a += data); // checksum byte
if (_payload_counter < UBLOX_PAYLOAD_SIZE) {
_buffer.bytes[_payload_counter] = data;
}
if (++_payload_counter >= _payload_length) {
_step++;
}
break;
case 7:
_step++;
if (_ck_a != data) {
_skip_packet = true; // bad checksum
gpsStats.errors++;
}
break;
case 8:
_step = 0;
if (_ck_b != data) {
gpsStats.errors++;
break; // bad checksum
}
gpsStats.packetCount++;
if (_skip_packet) {
break;
}
if (gpsParceFrameUBLOX()) {
parsed = true;
}
}
return parsed;
}
static bool gpsInitialize(void)
{
// UBLOX will be forced to the baud selected for the serial port. Autobaud will cycle thru autoBaudrateIndex, baudrateIndex is never changed
serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]);
// print our FIXED init string for the baudrate we want to be at
serialPrint(gpsState.gpsPort, baudInitData[gpsState.baudrateIndex]);
return false;
}
static bool gpsConfigure(void)
{
switch (gpsState.autoConfigStep) {
case 0: // Config
if (gpsState.autoConfigPosition < sizeof(ubloxInit6)) {
serialWrite(gpsState.gpsPort, ubloxInit6[gpsState.autoConfigPosition]);
gpsState.autoConfigPosition++;
}
else {
gpsState.autoConfigPosition = 0;
gpsState.autoConfigStep++;
}
break;
case 1: // SBAS
if (gpsState.autoConfigPosition < UBLOX_SBAS_MESSAGE_LENGTH) {
serialWrite(gpsState.gpsPort, ubloxSbas[gpsState.gpsConfig->sbasMode].message[gpsState.autoConfigPosition]);
gpsState.autoConfigPosition++;
} else {
gpsState.autoConfigPosition = 0;
gpsState.autoConfigStep++;
}
break;
default:
case 2:
// ublox should be initialised, try receiving
gpsSetState(GPS_RECEIVING_DATA);
break;
}
return false;
}
static bool gpsReceiveData(void)
{
bool hasNewData = false;
if (gpsState.gpsPort) {
while (serialRxBytesWaiting(gpsState.gpsPort)) {
uint8_t newChar = serialRead(gpsState.gpsPort);
if (gpsNewFrameUBLOX(newChar)) {
hasNewData = true;
}
}
}
return hasNewData;
}
bool gpsHandleUBLOX(void)
{
// Receive data
bool hasNewData = gpsReceiveData();
// Process state
switch(gpsState.state) {
default:
return false;
case GPS_INITIALIZING:
return gpsInitialize();
case GPS_CONFIGURE:
return gpsConfigure();
case GPS_RECEIVING_DATA:
return hasNewData;
}
}
#endif

2
src/main/io/serial_cli.c
View file

@ -322,7 +322,7 @@ static const char * const lookupTableAlignment[] = {
#ifdef GPS
static const char * const lookupTableGPSProvider[] = {
"NMEA", "UBLOX", "I2C"
"NMEA", "UBLOX", "I2CNAV"
};
static const char * const lookupTableGPSSBASMode[] = {

40
src/main/io/serial_msp.c
View file

@ -1101,18 +1101,18 @@ static bool processOutCommand(uint8_t cmdMSP)
case MSP_RAW_GPS:
headSerialReply(16);
serialize8(STATE(GPS_FIX));
serialize8(GPS_numSat);
serialize32(GPS_coord[LAT]);
serialize32(GPS_coord[LON]);
serialize16(GPS_altitude);
serialize16(GPS_speed);
serialize16(GPS_ground_course);
serialize8(gpsSol.numSat);
serialize32(gpsSol.llh.lat);
serialize32(gpsSol.llh.lon);
serialize16(gpsSol.llh.alt);
serialize16(gpsSol.groundSpeed);
serialize16(gpsSol.groundCourse);
break;
case MSP_COMP_GPS:
headSerialReply(5);
serialize16(GPS_distanceToHome);
serialize16(GPS_directionToHome);
serialize8(GPS_update & 1);
serialize8(gpsSol.flags.gpsHeartbeat ? 1 : 0);
break;
#ifdef NAV
case MSP_NAV_STATUS:
@ -1141,13 +1141,13 @@ static bool processOutCommand(uint8_t cmdMSP)
break;
#endif
case MSP_GPSSVINFO:
headSerialReply(1 + (GPS_numCh * 4));
serialize8(GPS_numCh);
for (i = 0; i < GPS_numCh; i++){
serialize8(GPS_svinfo_chn[i]);
serialize8(GPS_svinfo_svid[i]);
serialize8(GPS_svinfo_quality[i]);
serialize8(GPS_svinfo_cno[i]);
headSerialReply(1 + (gpsSol.numCh * 4));
serialize8(gpsSol.numCh);
for (i = 0; i < gpsSol.numCh; i++){
serialize8(gpsSol.svInfo[i].chn);
serialize8(gpsSol.svInfo[i].svid);
serialize8(gpsSol.svInfo[i].quality);
serialize8(gpsSol.svInfo[i].cno);
}
break;
#endif
@ -1631,14 +1631,14 @@ static bool processInCommand(void)
} else {
DISABLE_STATE(GPS_FIX);
}
GPS_numSat = read8();
GPS_coord[LAT] = read32();
GPS_coord[LON] = read32();
GPS_altitude = read16();
GPS_speed = read16();
gpsSol.numSat = read8();
gpsSol.llh.lat = read32();
gpsSol.llh.lon = read32();
gpsSol.llh.alt = read16();
gpsSol.groundSpeed = read16();
// Feed data to navigation
sensorsSet(SENSOR_GPS);
onNewGPSData(GPS_coord[LAT], GPS_coord[LON], GPS_altitude, 0, 0, 0, false, false, 9999);
onNewGPSData(gpsSol.llh.lat, gpsSol.llh.lon, gpsSol.llh.alt, 0, 0, 0, false, false, 9999);
break;
#endif
#ifdef NAV

7
src/main/main.c
View file

@ -114,6 +114,7 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
#endif
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions);
void gpsPreInit(gpsConfig_t *initialGpsConfig);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
void imuInit(void);
void displayInit(rxConfig_t *intialRxConfig);
@ -371,6 +372,12 @@ void init(void)
}
#endif
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsPreInit(&masterConfig.gpsConfig);
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf,
masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination,
masterConfig.looptime, masterConfig.gyroSync, masterConfig.gyroSyncDenominator)) {

2
src/main/mw.c
View file

@ -319,7 +319,7 @@ void mwArm(void)
//beep to indicate arming
#ifdef GPS
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5)
beeper(BEEPER_ARMING_GPS_FIX);
else
beeper(BEEPER_ARMING);

3
src/main/sensors/compass.h
View file

@ -23,7 +23,8 @@ typedef enum {
MAG_NONE = 1,
MAG_HMC5883 = 2,
MAG_AK8975 = 3,
MAG_FAKE = 4
MAG_GPS = 4,
MAG_FAKE = 5
} magSensor_e;
#define MAG_MAX MAG_FAKE

16
src/main/sensors/initialisation.c
View file

@ -72,6 +72,10 @@
#include "hardware_revision.h"
#endif
#ifdef GPS
extern bool gpsMagDetect(mag_t *mag);
#endif
extern float magneticDeclination;
extern gyro_t gyro;
@ -674,6 +678,18 @@ retry:
#endif
; // fallthrough
case MAG_GPS:
#ifdef GPS
if (gpsMagDetect(&mag)) {
#ifdef MAG_GPS_ALIGN
magAlign = MAG_GPS_ALIGN;
#endif
magHardware = MAG_GPS;
break;
}
#endif
; // fallthrough
case MAG_FAKE:
#ifdef USE_FAKE_MAG
if (fakeMagDetect(&mag)) {

4
src/main/target/ALIENWIIF3/target.h
View file

@ -113,6 +113,10 @@
//#define BLACKBOX
#define SERIAL_RX
//#define GPS
//#define GPS_PROTO_NMEA
//#define GPS_PROTO_UBLOX
//#define GPS_PROTO_I2C_NAV
#define GTUNE
//#define DISPLAY
#define USE_SERVOS

5
src/main/target/CC3D/target.h
View file

@ -108,7 +108,12 @@
#define RSSI_ADC_CHANNEL ADC_Channel_1
#define GPS
#define GPS_PROTO_NMEA
#define GPS_PROTO_UBLOX
#define GPS_PROTO_I2C_NAV
#define NAV
//#define LED_STRIP
//#define LED_STRIP_TIMER TIM3

4
src/main/target/CHEBUZZF3/target.h
View file

@ -99,6 +99,10 @@
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9
#define GPS
#define GPS_PROTO_NMEA
#define GPS_PROTO_UBLOX
#define GPS_PROTO_I2C_NAV
#define LED_STRIP
#if 1
#define LED_STRIP_TIMER TIM16

5
src/main/target/COLIBRI_RACE/target.h
View file

@ -143,7 +143,12 @@
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9
#define BLACKBOX
#define GPS
#define GPS_PROTO_NMEA
#define GPS_PROTO_UBLOX
#define GPS_PROTO_I2C_NAV
#define GTUNE
#define LED_STRIP

4
src/main/target/EUSTM32F103RC/target.h
View file

@ -118,6 +118,10 @@
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define GPS
#define GPS_PROTO_NMEA
#define GPS_PROTO_UBLOX
#define GPS_PROTO_I2C_NAV
#define LED_STRIP
#define LED_STRIP_TIMER TIM3

4
src/main/target/MOTOLAB/target.h
View file

@ -113,6 +113,10 @@
#define BLACKBOX
#define SERIAL_RX
//#define GPS
// #define GPS_PROTO_NMEA
// #define GPS_PROTO_UBLOX
// #define GPS_PROTO_I2C_NAV
#define DISPLAY
#define USE_SERVOS
#define USE_FLASHFS

4
src/main/target/NAZE/target.h
View file

@ -170,6 +170,10 @@
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define GPS
#define GPS_PROTO_NMEA
#define GPS_PROTO_UBLOX
#define GPS_PROTO_I2C_NAV
#define NAV
//#define GTUNE

5
src/main/target/NAZE32PRO/target.h
View file

@ -41,7 +41,12 @@
#define I2C_DEVICE (I2CDEV_1)
#define BLACKBOX
#define GPS
#define GPS_PROTO_NMEA
#define GPS_PROTO_UBLOX
#define GPS_PROTO_I2C_NAV
#define GTUNE
#define SERIAL_RX
#define TELEMETRY

4
src/main/target/OLIMEXINO/target.h
View file

@ -104,6 +104,10 @@
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define GPS
#define GPS_PROTO_NMEA
#define GPS_PROTO_UBLOX
#define GPS_PROTO_I2C_NAV
#define LED_STRIP
#define LED_STRIP_TIMER TIM3

5
src/main/target/PORT103R/target.h
View file

@ -149,7 +149,12 @@
#define LED_STRIP_TIMER TIM3
#define BLACKBOX
#define GPS
#define GPS_PROTO_NMEA
#define GPS_PROTO_UBLOX
#define GPS_PROTO_I2C_NAV
#define GTUNE
#define SERIAL_RX
#define TELEMETRY

5
src/main/target/RMDO/target.h
View file

@ -156,7 +156,12 @@
#define WS2811_IRQ DMA1_Channel2_IRQn
#define GPS
#define GPS_PROTO_NMEA
#define GPS_PROTO_UBLOX
#define GPS_PROTO_I2C_NAV
#define NAV
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX

7
src/main/target/SPARKY/target.h
View file

@ -108,8 +108,13 @@
#define CURRENT_METER_ADC_CHANNEL ADC_Channel_4
#define BLACKBOX
#define GPS
#define NAV
#define GPS
#define GPS_PROTO_NMEA
#define GPS_PROTO_UBLOX
#define GPS_PROTO_I2C_NAV
#define GTUNE
#define DISPLAY
#define SERIAL_RX

6
src/main/target/SPRACINGF3/target.h
View file

@ -160,8 +160,14 @@
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#define DISPLAY
#define GPS
#define GPS_PROTO_NMEA
#define GPS_PROTO_UBLOX
#define GPS_PROTO_I2C_NAV
#define NAV
#define GTUNE
#define SERIAL_RX
#define TELEMETRY

5
src/main/target/STM32F3DISCOVERY/target.h
View file

@ -90,7 +90,12 @@
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9
#define BLACKBOX
#define GPS
#define GPS_PROTO_NMEA
#define GPS_PROTO_UBLOX
#define GPS_PROTO_I2C_NAV
#define GTUNE
#define LED_STRIP
#define LED_STRIP_TIMER TIM16

17
src/main/telemetry/frsky.c
View file

@ -181,7 +181,7 @@ static void sendBaro(void)
#ifdef GPS
static void sendGpsAltitude(void)
{
uint16_t altitude = GPS_altitude;
uint16_t altitude = gpsSol.llh.alt;
//Send real GPS altitude only if it's reliable (there's a GPS fix)
if (!STATE(GPS_FIX)) {
altitude = 0;
@ -221,9 +221,9 @@ static void sendTemperature1(void)
#ifdef GPS
static void sendSatalliteSignalQualityAsTemperature2(void)
{
uint16_t satellite = GPS_numSat;
if (GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL);
uint16_t satellite = gpsSol.numSat;
if (gpsSol.hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
satellite = constrain(gpsSol.hdop, 0, GPS_MAX_HDOP_VAL);
}
sendDataHead(ID_TEMPRATURE2);
@ -245,9 +245,9 @@ static void sendSpeed(void)
//Speed should be sent in knots (GPS speed is in cm/s)
sendDataHead(ID_GPS_SPEED_BP);
//convert to knots: 1cm/s = 0.0194384449 knots
serialize16(GPS_speed * 1944 / 100000);
serialize16(gpsSol.groundSpeed * 1944 / 100000);
sendDataHead(ID_GPS_SPEED_AP);
serialize16((GPS_speed * 1944 / 100) % 100);
serialize16((gpsSol.groundSpeed * 1944 / 100) % 100);
}
#endif
@ -328,11 +328,14 @@ static void sendFakeLatLongThatAllowsHeadingDisplay(void)
static void sendGPSLatLong(void)
{
static uint8_t gpsFixOccured = 0;
int32_t coord[2] = {0,0};
if (STATE(GPS_FIX) || gpsFixOccured == 1) {
// If we have ever had a fix, send the last known lat/long
gpsFixOccured = 1;
sendLatLong(GPS_coord);
coord[LAT] = gpsSol.llh.lat;
coord[LON] = gpsSol.llh.lon;
sendLatLong(coord);
} else {
// otherwise send fake lat/long in order to display compass value
sendFakeLatLong();

10
src/main/telemetry/hott.c
View file

@ -178,30 +178,30 @@ void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t
void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
{
hottGPSMessage->gps_satelites = GPS_numSat;
hottGPSMessage->gps_satelites = gpsSol.numSat;
if (!STATE(GPS_FIX)) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
return;
}
if (GPS_numSat >= 5) {
if (gpsSol.numSat >= 5) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
} else {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D;
}
addGPSCoordinates(hottGPSMessage, GPS_coord[LAT], GPS_coord[LON]);
addGPSCoordinates(hottGPSMessage, gpsSol.llh.lat, gpsSol.llh.lon);
// GPS Speed is returned in cm/s (from io/gps.c) and must be sent in km/h (Hott requirement)
uint16_t speed = (GPS_speed * 36) / 1000;
uint16_t speed = (gpsSol.groundSpeed * 36) / 1000;
hottGPSMessage->gps_speed_L = speed & 0x00FF;
hottGPSMessage->gps_speed_H = speed >> 8;
hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF;
hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8;
uint16_t hottGpsAltitude = (GPS_altitude / 10) + HOTT_GPS_ALTITUDE_OFFSET; // 1 / 0.1f == 10, GPS_altitude of 1 == 0.1m
uint16_t hottGpsAltitude = (gpsSol.llh.alt / 10) + HOTT_GPS_ALTITUDE_OFFSET; // 1 / 0.1f == 10, GPS_altitude of 1 == 0.1m
hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF;
hottGPSMessage->altitude_H = hottGpsAltitude >> 8;

14
src/main/telemetry/ltm.c
View file

@ -137,23 +137,23 @@ static void ltm_gframe(void)
if (!STATE(GPS_FIX))
gps_fix_type = 1;
else if (GPS_numSat < 5)
else if (gpsSol.numSat < 5)
gps_fix_type = 2;
else
gps_fix_type = 3;
ltm_initialise_packet('G');
ltm_serialise_32(GPS_coord[LAT]);
ltm_serialise_32(GPS_coord[LON]);
ltm_serialise_8((uint8_t)(GPS_speed / 100));
ltm_serialise_32(gpsSol.llh.lat);
ltm_serialise_32(gpsSol.llh.lon);
ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100));
#if defined(NAV)
ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedActualPosition(Z) : GPS_altitude * 100;
ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedActualPosition(Z) : gpsSol.llh.alt * 100;
#else
ltm_alt = GPS_altitude * 100;
ltm_alt = gpsSol.llh.alt * 100;
#endif
ltm_serialise_32(ltm_alt);
ltm_serialise_8((GPS_numSat << 2) | gps_fix_type);
ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type);
ltm_finalise();
}
#endif

14
src/main/telemetry/smartport.c
View file

@ -316,7 +316,7 @@ void handleSmartPortTelemetry(void)
#ifdef GPS
case FSSP_DATAID_SPEED :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100;
uint32_t tmpui = (gpsSol.groundSpeed * 36 + 36 / 2) / 100;
smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H
smartPortHasRequest = 0;
}
@ -357,14 +357,14 @@ void handleSmartPortTelemetry(void)
// the MSB of the sent uint32_t helps FrSky keep track
// the even/odd bit of our counter helps us keep track
if (smartPortIdCnt & 1) {
tmpui = abs(GPS_coord[LON]); // now we have unsigned value and one bit to spare
tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare
tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast
if (GPS_coord[LON] < 0) tmpui |= 0x40000000;
if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
}
else {
tmpui = abs(GPS_coord[LAT]); // now we have unsigned value and one bit to spare
tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare
tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast
if (GPS_coord[LAT] < 0) tmpui |= 0x40000000;
if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
}
smartPortSendPackage(id, tmpui);
smartPortHasRequest = 0;
@ -446,7 +446,7 @@ void handleSmartPortTelemetry(void)
if (sensors(SENSOR_GPS)) {
#ifdef GPS
// provide GPS lock status
smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat);
smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + gpsSol.numSat);
smartPortHasRequest = 0;
#endif
}
@ -458,7 +458,7 @@ void handleSmartPortTelemetry(void)
#ifdef GPS
case FSSP_DATAID_GPS_ALT :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
smartPortSendPackage(id, gpsSol.llh.alt * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
smartPortHasRequest = 0;
}
break;