Skip to content

Commit d5108f3

Browse files
digitalentitydigitalentity
committed
[GPS] Add MSP_GPS sensor. Enable on F4+ boards; Add extra RTC sanity check
1 parent 5967029 commit d5108f3

File tree

9 files changed

+185
-14
lines changed

9 files changed

+185
-14
lines changed

‎make/source.mk‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -201,6 +201,7 @@ MAIN_SRC = \
201201
io/gps_ublox.c \
202202
io/gps_nmea.c \
203203
io/gps_naza.c \
204+
io/gps_msp.c \
204205
io/ledstrip.c \
205206
io/osd.c \
206207
io/osd_canvas.c \

‎src/main/fc/fc_msp.c‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3211,6 +3211,24 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
32113211
mspOpflowReceiveNewData(sbufPtr(src));
32123212
break;
32133213
#endif
3214+
3215+
#if defined(USE_GPS_PROTO_MSP)
3216+
case MSP2_SENSOR_GPS:
3217+
mspGPSReceiveNewData(sbufPtr(src));
3218+
break;
3219+
#endif
3220+
3221+
#if defined(USE_COMPASS_MSP)
3222+
case MSP2_SENSOR_COMPASS:
3223+
mspCompassReceiveNewData(sbufPtr(src));
3224+
break;
3225+
#endif
3226+
3227+
#if defined(USE_BARO_MSP)
3228+
case MSP2_SENSOR_BAROMETER:
3229+
mspBaroReceiveNewData(sbufPtr(src));
3230+
break;
3231+
#endif
32143232
}
32153233

32163234
return MSP_RESULT_NO_REPLY;

‎src/main/fc/settings.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ tables:
4444
values: ["NONE", "ADC", "ESC"]
4545
enum: voltageSensor_e
4646
- name: gps_provider
47-
values: ["NMEA", "UBLOX", "UNUSED", "NAZA", "UBLOX7", "MTK"]
47+
values: ["NMEA", "UBLOX", "UNUSED", "NAZA", "UBLOX7", "MTK", "MSP"]
4848
enum: gpsProvider_e
4949
- name: gps_sbas_mode
5050
values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "NONE"]

‎src/main/io/gps.c‎

Lines changed: 25 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,7 @@
6060
#include "fc/runtime_config.h"
6161

6262
typedef struct {
63+
bool isDriverBased;
6364
portMode_t portMode; // Port mode RX/TX (only for serial based)
6465
bool hasCompass; // Has a compass (NAZA)
6566
void (*restart)(void); // Restart protocol driver thread
@@ -77,42 +78,48 @@ baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600,
7778
static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = {
7879
/* NMEA GPS */
7980
#ifdef USE_GPS_PROTO_NMEA
80-
{ MODE_RX, false, &gpsRestartNMEA_MTK, &gpsHandleNMEA },
81+
{ false, MODE_RX, false, &gpsRestartNMEA_MTK, &gpsHandleNMEA },
8182
#else
82-
{ 0, false, NULL, NULL },
83+
{ false, 0, false, NULL, NULL },
8384
#endif
8485

8586
/* UBLOX binary */
8687
#ifdef USE_GPS_PROTO_UBLOX
87-
{ MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
88+
{ false, MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
8889
#else
89-
{ 0, false, NULL, NULL },
90+
{ false, 0, false, NULL, NULL },
9091
#endif
9192

9293
/* Stub */
93-
{ 0, false, NULL, NULL },
94+
{ false, 0, false, NULL, NULL },
9495

9596
/* NAZA GPS module */
9697
#ifdef USE_GPS_PROTO_NAZA
97-
{ MODE_RX, true, &gpsRestartNAZA, &gpsHandleNAZA },
98+
{ false, MODE_RX, true, &gpsRestartNAZA, &gpsHandleNAZA },
9899
#else
99-
{ 0, false, NULL, NULL },
100+
{ false, 0, false, NULL, NULL },
100101
#endif
101102

102103
/* UBLOX7PLUS binary */
103104
#ifdef USE_GPS_PROTO_UBLOX
104-
{ MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
105+
{ false, MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
105106
#else
106-
{ 0, false, NULL, NULL },
107+
{ false, 0, false, NULL, NULL },
107108
#endif
108109

109110
/* MTK GPS */
110111
#ifdef USE_GPS_PROTO_MTK
111-
{ MODE_RXTX, false, &gpsRestartNMEA_MTK, &gpsHandleMTK },
112+
{ false, MODE_RXTX, false, &gpsRestartNMEA_MTK, &gpsHandleMTK },
112113
#else
113-
{ 0, false, NULL, NULL },
114+
{ false, 0, false, NULL, NULL },
114115
#endif
115116

117+
/* MSP GPS */
118+
#ifdef USE_GPS_PROTO_MSP
119+
{ true, 0, false, &gpsRestartMSP, &gpsHandleMSP },
120+
#else
121+
{ false, 0, false, NULL, NULL },
122+
#endif
116123
};
117124

118125
PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
@@ -135,7 +142,7 @@ void gpsSetState(gpsState_e state)
135142

136143
static void gpsUpdateTime(void)
137144
{
138-
if (!rtcHasTime() && gpsSol.flags.validTime) {
145+
if (!rtcHasTime() && gpsSol.flags.validTime && gpsSol.time.year != 0) {
139146
rtcSetDateTime(&gpsSol.time);
140147
}
141148
}
@@ -220,6 +227,12 @@ void gpsInit(void)
220227
return;
221228
}
222229

230+
// Shortcut for driver-based GPS (MSP)
231+
if (gpsProviders[gpsState.gpsConfig->provider].isDriverBased) {
232+
gpsSetState(GPS_INITIALIZING);
233+
return;
234+
}
235+
223236
serialPortConfig_t * gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
224237
if (!gpsPortConfig) {
225238
featureClear(FEATURE_GPS);

‎src/main/io/gps.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,7 @@ typedef enum {
3838
GPS_NAZA,
3939
GPS_UBLOX7PLUS,
4040
GPS_MTK,
41+
GPS_MSP,
4142
GPS_PROVIDER_COUNT
4243
} gpsProvider_e;
4344

@@ -163,3 +164,4 @@ bool isGPSHealthy(void);
163164
bool isGPSHeadingValid(void);
164165
struct serialPort_s;
165166
void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
167+
void mspGPSReceiveNewData(uint8_t * bufferPtr);

‎src/main/io/gps_msp.c‎

Lines changed: 130 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,130 @@
1+
/*
2+
* This file is part of Cleanflight.
3+
*
4+
* Cleanflight is free software: you can redistribute it and/or modify
5+
* it under the terms of the GNU General Public License as published by
6+
* the Free Software Foundation, either version 3 of the License, or
7+
* (at your option) any later version.
8+
*
9+
* Cleanflight is distributed in the hope that it will be useful,
10+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
11+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12+
* GNU General Public License for more details.
13+
*
14+
* You should have received a copy of the GNU General Public License
15+
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
16+
*/
17+
18+
#include <stdbool.h>
19+
#include <stdint.h>
20+
#include <ctype.h>
21+
#include <string.h>
22+
#include <math.h>
23+
24+
#include "platform.h"
25+
#include "build/build_config.h"
26+
27+
28+
#if defined(USE_GPS_PROTO_MSP)
29+
30+
#include "build/debug.h"
31+
32+
#include "common/axis.h"
33+
#include "common/gps_conversion.h"
34+
#include "common/maths.h"
35+
#include "common/utils.h"
36+
37+
#include "drivers/serial.h"
38+
#include "drivers/time.h"
39+
40+
#include "fc/config.h"
41+
#include "fc/runtime_config.h"
42+
43+
#include "io/gps.h"
44+
#include "io/gps_private.h"
45+
#include "io/serial.h"
46+
47+
#include "scheduler/protothreads.h"
48+
49+
typedef struct __attribute__((packed)) {
50+
uint32_t msTOW;
51+
uint8_t fixType;
52+
uint8_t satellitesInView;
53+
uint16_t horizontalPosAccuracy; // [cm]
54+
uint16_t verticalPosAccuracy; // [cm]
55+
uint16_t horizontalVelAccuracy; // [cm/s]
56+
uint16_t hdop;
57+
int32_t longitude;
58+
int32_t latitude;
59+
int32_t mslAltitude; // cm
60+
int32_t nedVelNorth; // cm/s
61+
int32_t nedVelEast;
62+
int32_t nedVelDown;
63+
int16_t groundCourse; // deg * 100
64+
int16_t trueYaw; // deg * 100, values of 0..36000 are valid. 65535 = no data available
65+
uint16_t year;
66+
uint8_t month;
67+
uint8_t day;
68+
uint8_t hour;
69+
uint8_t min;
70+
uint8_t sec;
71+
} mspGpsDataMessage_t;
72+
73+
static bool newDataReady;
74+
75+
void gpsRestartMSP(void)
76+
{
77+
// NOP
78+
}
79+
80+
void gpsHandleMSP(void)
81+
{
82+
if (newDataReady) {
83+
gpsProcessNewSolutionData();
84+
newDataReady = false;
85+
}
86+
}
87+
88+
static uint8_t gpsMapFixType(uint8_t mspFixType)
89+
{
90+
if (mspFixType == 2)
91+
return GPS_FIX_2D;
92+
if (mspFixType >= 3)
93+
return GPS_FIX_3D;
94+
return GPS_NO_FIX;
95+
}
96+
97+
void mspGPSReceiveNewData(uint8_t * bufferPtr)
98+
{
99+
mspGpsDataMessage_t * pkt = (mspGpsDataMessage_t *)bufferPtr;
100+
101+
gpsSol.fixType = gpsMapFixType(pkt->fixType);
102+
gpsSol.numSat = pkt->satellitesInView;
103+
gpsSol.llh.lon = pkt->longitude;
104+
gpsSol.llh.lat = pkt->latitude;
105+
gpsSol.llh.alt = pkt->mslAltitude;
106+
gpsSol.velNED[X] = pkt->nedVelNorth;
107+
gpsSol.velNED[Y] = pkt->nedVelEast;
108+
gpsSol.velNED[Z] = pkt->nedVelDown;
109+
gpsSol.groundSpeed = sqrtf(sq((float)pkt->nedVelNorth) + sq((float)pkt->nedVelEast));
110+
gpsSol.groundCourse = pkt->groundCourse / 10; // in deg * 10
111+
gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
112+
gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
113+
gpsSol.hdop = gpsConstrainHDOP(pkt->hdop);
114+
gpsSol.flags.validVelNE = 1;
115+
gpsSol.flags.validVelD = 1;
116+
gpsSol.flags.validEPE = 1;
117+
118+
gpsSol.time.year = pkt->year;
119+
gpsSol.time.month = pkt->month;
120+
gpsSol.time.day = pkt->day;
121+
gpsSol.time.hours = pkt->hour;
122+
gpsSol.time.minutes = pkt->min;
123+
gpsSol.time.seconds = pkt->sec;
124+
gpsSol.time.millis = 0;
125+
126+
gpsSol.flags.validTime = (pkt->fixType >= 3);
127+
128+
newDataReady = true;
129+
}
130+
#endif

‎src/main/io/gps_private.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -78,4 +78,7 @@ extern void gpsHandleMTK(void);
7878
extern void gpsRestartNAZA(void);
7979
extern void gpsHandleNAZA(void);
8080

81+
extern void gpsRestartMSP(void);
82+
extern void gpsHandleMSP(void);
83+
8184
#endif

‎src/main/msp/msp_protocol_v2_sensor.h‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,4 +18,7 @@
1818
#define MSP2_IS_SENSOR_MESSAGE(x) ((x) >= 0x1F00 && (x) <= 0x1FFF)
1919

2020
#define MSP2_SENSOR_RANGEFINDER 0x1F01
21-
#define MSP2_SENSOR_OPTIC_FLOW 0x1F02
21+
#define MSP2_SENSOR_OPTIC_FLOW 0x1F02
22+
#define MSP2_SENSOR_GPS 0x1F03
23+
#define MSP2_SENSOR_COMPASS 0x1F04
24+
#define MSP2_SENSOR_BAROMETER 0x1F05

‎src/main/target/common.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,7 @@
5555
#define USE_BLACKBOX
5656
#define USE_GPS
5757
#define USE_GPS_PROTO_UBLOX
58+
#define USE_GPS_PROTO_MSP
5859
#define USE_NAV
5960
#define USE_TELEMETRY
6061
#define USE_TELEMETRY_LTM

0 commit comments

Comments
 (0)