Skip to content

Commit 008e61a

Browse files
JcZouJcZou
committed
Resolve conflicts
2 parents dd5225b + 8851e09 commit 008e61a

File tree

16 files changed

+769
-39
lines changed

16 files changed

+769
-39
lines changed

‎src/driver/gps/gps_nmea.c‎

Lines changed: 208 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,208 @@
1+
/******************************************************************************
2+
* Copyright The Firmament Authors. All Rights Reserved.
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*****************************************************************************/
16+
#include <firmament.h>
17+
18+
#include "driver/gps/gps_nmea.h"
19+
#include "hal/gps/gps.h"
20+
#include "hal/serial/serial.h"
21+
#include "module/sensor/sensor_hub.h"
22+
#include "protocol/nmea/nmea.h"
23+
24+
static rt_device_t serial_device;
25+
static struct gps_device gps_device;
26+
static nmea_decoder_t nmea_decoder;
27+
static gps_report_t gps_report;
28+
static bool nmea_received;
29+
30+
static rt_err_t gps_serial_rx_ind(rt_device_t dev, rt_size_t size)
31+
{
32+
uint8_t ch;
33+
34+
while (rt_device_read(serial_device, 0, &ch, 1)) {
35+
parse_nmea_char(&nmea_decoder, ch);
36+
}
37+
38+
return RT_EOK;
39+
}
40+
41+
static int nmea_rx_handle(uint16_t msg_id)
42+
{
43+
int ret = 0;
44+
45+
switch (msg_id) {
46+
case NMEA_MSG_KSXT: {
47+
gps_report.vel_ned_valid = 1;
48+
if (nmea_decoder.buf.payload_rx_ksxt.pos_fixType == 1) {
49+
gps_report.fix_type = 3; /* 3D fix */
50+
} else if (nmea_decoder.buf.payload_rx_ksxt.pos_fixType == 2) {
51+
gps_report.fix_type = 6; /* RTK fix */
52+
} else if (nmea_decoder.buf.payload_rx_ksxt.pos_fixType == 3) {
53+
gps_report.fix_type = 5; /* RTK float */
54+
} else {
55+
gps_report.fix_type = 0; /* No fix */
56+
gps_report.vel_ned_valid = 0;
57+
}
58+
59+
gps_report.satellites_used = nmea_decoder.buf.payload_rx_ksxt.MnumSV;
60+
gps_report.lat = nmea_decoder.buf.payload_rx_ksxt.lat;
61+
gps_report.lon = nmea_decoder.buf.payload_rx_ksxt.lon;
62+
gps_report.alt = nmea_decoder.buf.payload_rx_ksxt.height;
63+
gps_report.vel_m_s = nmea_decoder.buf.payload_rx_ksxt.gSpeed;
64+
gps_report.vel_n_m_s = nmea_decoder.buf.payload_rx_ksxt.velN;
65+
gps_report.vel_e_m_s = nmea_decoder.buf.payload_rx_ksxt.velE;
66+
gps_report.vel_d_m_s = nmea_decoder.buf.payload_rx_ksxt.velD;
67+
68+
gps_report.heading_rad = nmea_decoder.buf.payload_rx_ksxt.heading;
69+
gps_report.cog_rad = nmea_decoder.buf.payload_rx_ksxt.track;
70+
gps_report.c_variance_rad = (3.0f - nmea_decoder.buf.payload_rx_ksxt.heading_fixType) * PI / 6.0f;
71+
72+
gps_report.timestamp_time = systime_now_ms();
73+
gps_report.timestamp_velocity = systime_now_ms();
74+
gps_report.timestamp_position = systime_now_ms();
75+
76+
nmea_decoder.got_posllh = true;
77+
nmea_decoder.got_velned = true;
78+
nmea_decoder.got_svinfo = true;
79+
80+
} break;
81+
case NMEA_MSG_GPGSA: {
82+
gps_report.eph = nmea_decoder.buf.payload_rx_gpgsa.hdop;
83+
gps_report.epv = nmea_decoder.buf.payload_rx_gpgsa.vdop;
84+
gps_report.s_variance_m_s = nmea_decoder.buf.payload_rx_gpgsa.hdop / 5;
85+
86+
gps_report.timestamp_variance = systime_now_ms();
87+
88+
nmea_decoder.got_dop = true;
89+
} break;
90+
default:
91+
ret = -1;
92+
break;
93+
}
94+
95+
nmea_received = true;
96+
97+
return ret;
98+
}
99+
100+
static void gps_probe_entry(void* parameter)
101+
{
102+
char nmea_cfg[][50] = {
103+
"KSXT 0.1\r\n",
104+
"GPGSA 0.1\r\n",
105+
};
106+
107+
nmea_received = false;
108+
109+
for (uint8_t i = 0; i < sizeof(nmea_cfg) / sizeof(nmea_cfg[0]); i++) {
110+
rt_device_write(nmea_decoder.nmea_dev, 0, nmea_cfg[i], strlen(nmea_cfg[i]));
111+
systime_msleep(10);
112+
}
113+
114+
uint32_t time_now = systime_now_ms();
115+
while((systime_now_ms() - time_now) < 5000) {
116+
if(nmea_received){
117+
printf("NMEA GPS detected.\n");
118+
break;
119+
}
120+
systime_msleep(10);
121+
}
122+
123+
/* GPS is dected, now register */
124+
hal_gps_register(&gps_device, "gps", RT_DEVICE_FLAG_RDWR, RT_NULL);
125+
register_sensor_gps((char*)parameter);
126+
127+
rt_free(parameter);
128+
}
129+
130+
static rt_err_t gps_control(gps_dev_t gps_dev, int cmd, void* arg)
131+
{
132+
switch (cmd) {
133+
case GPS_CMD_CHECK_READY:
134+
if (arg == NULL) {
135+
return RT_ERROR;
136+
}
137+
if (nmea_decoder.got_posllh && nmea_decoder.got_velned && nmea_decoder.got_svinfo && nmea_decoder.got_dop) {
138+
*(uint8_t*)arg = 1;
139+
} else {
140+
*(uint8_t*)arg = 0;
141+
}
142+
return RT_EOK;
143+
144+
break;
145+
146+
case GPS_CMD_INIT:
147+
case GPS_CMD_OPEN:
148+
case GPS_CMD_CLOSE:
149+
return RT_EOK;
150+
151+
break;
152+
}
153+
154+
return RT_EINVAL;
155+
}
156+
157+
static rt_size_t gps_read(gps_dev_t gps_dev, gps_report_t* report)
158+
{
159+
if (nmea_decoder.got_posllh && nmea_decoder.got_velned && nmea_decoder.got_svinfo && nmea_decoder.got_dop) {
160+
161+
OS_ENTER_CRITICAL;
162+
*report = gps_report;
163+
nmea_decoder.got_posllh = false;
164+
nmea_decoder.got_velned = false;
165+
nmea_decoder.got_svinfo = false;
166+
nmea_decoder.got_dop = false;
167+
OS_EXIT_CRITICAL;
168+
169+
return sizeof(gps_report_t);
170+
}
171+
172+
return 0;
173+
}
174+
175+
static struct gps_ops gps_ops = {
176+
.gps_control = gps_control,
177+
.gps_read = gps_read
178+
};
179+
180+
rt_err_t gps_nmea_init(const char* serial_dev_name, const char* gps_dev_name)
181+
{
182+
char* str_buffer;
183+
184+
gps_device.ops = &gps_ops;
185+
186+
str_buffer = (char*)rt_malloc(21);
187+
RT_ASSERT(str_buffer != NULL);
188+
memset(str_buffer, 0, 21);
189+
strncpy(str_buffer, gps_dev_name, 20);
190+
191+
serial_device = rt_device_find(serial_dev_name);
192+
RT_ASSERT(serial_device != NULL);
193+
194+
/* set gps rx indicator */
195+
RT_CHECK(rt_device_set_rx_indicate(serial_device, gps_serial_rx_ind));
196+
/* open serial device */
197+
RT_CHECK(rt_device_open(serial_device, RT_DEVICE_OFLAG_RDWR | RT_DEVICE_FLAG_DMA_RX | RT_DEVICE_FLAG_DMA_TX));
198+
/* init ublox decoder */
199+
FMT_CHECK(init_nmea_decoder(&nmea_decoder, serial_device, nmea_rx_handle));
200+
201+
/* create a thread to probe the gps connection */
202+
rt_thread_t tid = rt_thread_create("gps_probe", gps_probe_entry, str_buffer, 4096, RT_THREAD_PRIORITY_MAX - 2, 5);
203+
RT_ASSERT(tid != NULL);
204+
205+
RT_CHECK(rt_thread_startup(tid));
206+
207+
return RT_EOK;
208+
}

‎src/driver/gps/gps_nmea.h‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
/******************************************************************************
2+
* Copyright The Firmament Authors. All Rights Reserved.
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*****************************************************************************/
16+
17+
#ifndef GPS_NMEA_H__
18+
#define GPS_NMEA_H__
19+
20+
#include <rtthread.h>
21+
22+
#ifdef __cplusplus
23+
extern "C" {
24+
#endif
25+
26+
rt_err_t gps_nmea_init(const char* serial_device_name, const char* gps_device_name);
27+
28+
#ifdef __cplusplus
29+
}
30+
#endif
31+
32+
#endif

‎src/driver/gps/gps_ubx.c‎

Lines changed: 8 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -93,7 +93,7 @@ static int ubx_rx_handle(void)
9393
gps_report.vel_d_m_s = (float)ubx_decoder.buf.payload_rx_nav_pvt.velD * 1e-3f;
9494

9595
gps_report.cog_rad = (float)ubx_decoder.buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f;
96-
gps_report.c_variance_rad = (float)ubx_decoder.buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f;
96+
gps_report.c_variance_rad = 2 * PI;
9797

9898
// Check if time and date fix flags are good
9999
// if ((ubx_decoder.buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE)
@@ -202,7 +202,7 @@ static int ubx_rx_handle(void)
202202
gps_report.vel_e_m_s = (float)ubx_decoder.buf.payload_rx_nav_velned.velE * 1e-2f; /* NED EAST velocity */
203203
gps_report.vel_d_m_s = (float)ubx_decoder.buf.payload_rx_nav_velned.velD * 1e-2f; /* NED DOWN velocity */
204204
gps_report.cog_rad = (float)ubx_decoder.buf.payload_rx_nav_velned.heading * M_DEG_TO_RAD_F * 1e-5f;
205-
gps_report.c_variance_rad = (float)ubx_decoder.buf.payload_rx_nav_velned.cAcc * M_DEG_TO_RAD_F * 1e-5f;
205+
gps_report.c_variance_rad = 2 * PI;
206206
gps_report.vel_ned_valid = 1;
207207

208208
gps_report.timestamp_velocity = systime_now_ms();
@@ -514,27 +514,25 @@ static struct gps_ops gps_ops = {
514514

515515
static void gps_probe_entry(void* parameter)
516516
{
517-
#ifdef FMT_SKIP_GPS_UBX_CONFIG
518-
ubx_decoder.use_nav_pvt = true;
519-
ubx_decoder.configured = true;
520-
#else
521517
uint32_t baudrate;
522518
uint8_t i;
519+
523520
for (i = 0; i < CONFIGURE_RETRY_MAX; i++) {
524521
if (probe(&baudrate) == RT_EOK) {
525522
if (configure_by_ubx(baudrate) == RT_EOK) {
526-
// /* GPS is dected, now register */
527-
// hal_gps_register(&gps_device, "gps", RT_DEVICE_FLAG_RDWR, RT_NULL);
528-
// register_sensor_gps((char*)parameter);
523+
printf("UBX GPS detected.\n");
529524
break;
530525
}
531526
}
532527
}
533528

534529
if (i >= CONFIGURE_RETRY_MAX) {
535530
printf("GPS configuration fail! Please check if GPS module has connected.");
531+
532+
/* If configure failed, skip configure step and just wait pvt msg. since some ublox gps doesn't support ubx configure */
533+
ubx_decoder.use_nav_pvt = true;
534+
ubx_decoder.configured = true;
536535
}
537-
#endif
538536

539537
if (ubx_decoder.configured) {
540538
/* GPS is dected, now register */

‎src/driver/gps/gps_ubx.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,8 @@
1414
* limitations under the License.
1515
*****************************************************************************/
1616

17-
#ifndef GPS_MBX_H__
18-
#define GPS_MBX_H__
17+
#ifndef GPS_UBX_H__
18+
#define GPS_UBX_H__
1919

2020
#include <rtthread.h>
2121

‎src/hal/gps/gps.h‎

Lines changed: 14 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ extern "C" {
2424
#endif
2525

2626
/* gps read pos */
27-
#define GPS_READ_REPORT 0
27+
#define GPS_READ_REPORT 0
2828

2929
/* gps command */
3030
#define GPS_CMD_INIT 0x10
@@ -39,28 +39,29 @@ typedef struct {
3939
int32_t alt; /**< Altitude in 1E-3 meters (millimeters) above MSL */
4040

4141
uint32_t timestamp_variance;
42-
float s_variance_m_s; /**< speed accuracy estimate m/s */
43-
float c_variance_rad; /**< course accuracy estimate rad */
44-
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
42+
float s_variance_m_s; /**< speed accuracy estimate m/s */
43+
float c_variance_rad; /**< course accuracy estimate rad */
44+
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
4545

46-
float eph; /**< GPS HDOP horizontal dilution of position in m */
47-
float epv; /**< GPS VDOP horizontal dilution of position in m */
46+
float eph; /**< GPS HDOP horizontal dilution of position in m */
47+
float epv; /**< GPS VDOP horizontal dilution of position in m */
4848

49-
uint16_t noise_per_ms; /**< */
50-
uint16_t jamming_indicator; /**< */
49+
uint16_t noise_per_ms; /**< */
50+
uint16_t jamming_indicator; /**< */
5151

5252
uint32_t timestamp_velocity; /**< Timestamp for velocity informations */
5353
float vel_m_s; /**< GPS ground speed (m/s) */
5454
float vel_n_m_s; /**< North velocity in m/s */
5555
float vel_e_m_s; /**< East velocity in m/s */
5656
float vel_d_m_s; /**< Down velocity in m/s */
57+
float heading_rad; /**< Heading in rad, -PI..PI */
5758
float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
5859
uint8_t vel_ned_valid; /**< Flag to indicate if NED speed is valid */
5960

60-
uint32_t timestamp_time; /**< Timestamp for time information */
61-
uint32_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */
61+
uint32_t timestamp_time; /**< Timestamp for time information */
62+
uint32_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */
6263

63-
uint8_t satellites_used; /**< Number of satellites used */
64+
uint8_t satellites_used; /**< Number of satellites used */
6465
float hdop;
6566
float ndop;
6667
float edop;
@@ -82,13 +83,13 @@ struct gps_ops {
8283
* @param dev gps device
8384
* @param cmd operation command
8485
* @param arg command argument
85-
*/
86+
*/
8687
rt_err_t (*gps_control)(gps_dev_t dev, int cmd, void* arg);
8788
/**
8889
* @brief gps read function
8990
* @param dev gps device
9091
* @param report gps report buffer
91-
*/
92+
*/
9293
rt_size_t (*gps_read)(gps_dev_t dev, gps_report_t* report);
9394
};
9495

‎src/model/ins/cf_ins/ins_interface.c‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -427,9 +427,11 @@ void ins_interface_step(uint32_t timestamp)
427427
INS_U.GPS_uBlox.velN = (int32_t)(ins_handle.gps_report.velN * 1e3);
428428
INS_U.GPS_uBlox.velE = (int32_t)(ins_handle.gps_report.velE * 1e3);
429429
INS_U.GPS_uBlox.velD = (int32_t)(ins_handle.gps_report.velD * 1e3);
430+
INS_U.GPS_uBlox.heading = (int32_t)(ins_handle.gps_report.heading * 1e3);
430431
INS_U.GPS_uBlox.hAcc = (uint32_t)(ins_handle.gps_report.hAcc * 1e3);
431432
INS_U.GPS_uBlox.vAcc = (uint32_t)(ins_handle.gps_report.vAcc * 1e3);
432433
INS_U.GPS_uBlox.sAcc = (uint32_t)(ins_handle.gps_report.sAcc * 1e3);
434+
INS_U.GPS_uBlox.headingAcc = (uint32_t)(ins_handle.gps_report.headingAcc * 1e3);
433435
INS_U.GPS_uBlox.numSV = ins_handle.gps_report.numSV;
434436
INS_U.GPS_uBlox.timestamp = timestamp;
435437

‎src/module/sensor/sensor_gps.c‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,8 +55,10 @@ fmt_err_t sensor_gps_read(sensor_gps_t gps_dev, gps_data_t* gps_data)
5555
gps_data->velE = gps_drv_report.vel_e_m_s;
5656
gps_data->velD = gps_drv_report.vel_d_m_s;
5757
gps_data->vel = gps_drv_report.vel_m_s;
58-
gps_data->cog = gps_drv_report.cog_rad;
5958
gps_data->sAcc = gps_drv_report.s_variance_m_s;
59+
gps_data->cog = gps_drv_report.cog_rad;
60+
gps_data->heading = gps_drv_report.heading_rad;
61+
gps_data->headingAcc = gps_drv_report.c_variance_rad;
6062

6163
return (r_size == sizeof(gps_report_t)) ? FMT_EOK : FMT_ERROR;
6264
}

0 commit comments

Comments
 (0)