mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 16:55:29 +03:00
GPS: Protocol-independent stack. UBLOX binary driver implementation. Autoconfig works. Autobaud works.
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)
parent
a5628cc4a8
commit
69cfbb04d2
38 changed files with 1491 additions and 1249 deletions
3
Makefile
3
Makefile
|
|
@ -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 \
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
1329
src/main/io/gps.c
1329
src/main/io/gps.c
File diff suppressed because it is too large
Load diff
|
|
@ -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
|
||||
uint8_t numSat;
|
||||
|
||||
uint8_t numCh;
|
||||
gpsSVChannel_t svInfo[GPS_SV_MAXSATS];
|
||||
|
||||
gpsLocation_t llh;
|
||||
int16_t magData[3];
|
||||
int16_t velNED[3];
|
||||
|
||||
int16_t groundSpeed;
|
||||
int16_t groundCourse;
|
||||
|
||||
uint16_t hdop;
|
||||
uint16_t vdop;
|
||||
} gpsSolutionData_t;
|
||||
|
||||
typedef struct {
|
||||
uint16_t lastMessageDt;
|
||||
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.
|
||||
|
||||
uint32_t state_position; // incremental variable for loops
|
||||
uint32_t state_ts; // timestamp for last state_position increment
|
||||
gpsMessageState_e messageState;
|
||||
} gpsData_t;
|
||||
|
||||
#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];
|
||||
|
||||
extern gpsData_t gpsData;
|
||||
extern int32_t GPS_coord[2]; // LAT/LON
|
||||
|
||||
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)
|
||||
|
||||
#define GPS_DBHZ_MIN 0
|
||||
#define GPS_DBHZ_MAX 55
|
||||
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
125
src/main/io/gps_i2cnav.c
Executable 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
331
src/main/io/gps_nmea.c
Executable 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
57
src/main/io/gps_private.h
Executable 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
512
src/main/io/gps_ublox.c
Executable 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
|
||||
|
|
@ -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[] = {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)) {
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue