projects
/
fw
/
altos
/ blobdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
|
commitdiff
|
tree
raw
|
inline
| side by side
altos/stmf0: Switch ADC fast to ADC clock
[fw/altos]
/
src
/
drivers
/
ao_gps_ublox.c
diff --git
a/src/drivers/ao_gps_ublox.c
b/src/drivers/ao_gps_ublox.c
index a11ca3f731657f7351b3759ac85bdd09d7a4f32b..22af413a0098008f2ed7dd1e1fa44834ef0c54f9 100644
(file)
--- a/
src/drivers/ao_gps_ublox.c
+++ b/
src/drivers/ao_gps_ublox.c
@@
-3,7
+3,8
@@
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
*
* This program 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; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
@@
-21,30
+22,37
@@
#include "ao_gps_ublox.h"
#include "ao_gps_ublox.h"
-#define AO_UBLOX_DEBUG
1
+#define AO_UBLOX_DEBUG
0
#include <stdarg.h>
#include <stdarg.h>
+__xdata uint8_t ao_gps_new;
__xdata uint8_t ao_gps_mutex;
__pdata uint16_t ao_gps_tick;
__xdata struct ao_telemetry_location ao_gps_data;
__xdata struct ao_telemetry_satellite ao_gps_tracking_data;
__xdata uint8_t ao_gps_mutex;
__pdata uint16_t ao_gps_tick;
__xdata struct ao_telemetry_location ao_gps_data;
__xdata struct ao_telemetry_satellite ao_gps_tracking_data;
+#undef AO_SERIAL_SPEED_UBLOX
+
#ifndef AO_SERIAL_SPEED_UBLOX
#ifndef AO_SERIAL_SPEED_UBLOX
-#define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_
57
600
+#define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_
9
600
#endif
#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600
#define SERIAL_SPEED_STRING "57600"
#endif
#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600
#define SERIAL_SPEED_STRING "57600"
+#define SERIAL_SPEED_CHECKSUM "2d"
#endif
#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200
#define SERIAL_SPEED_STRING "19200"
#endif
#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200
#define SERIAL_SPEED_STRING "19200"
+#define SERIAL_SPEED_CHECKSUM "23"
#endif
#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600
#define SERIAL_SPEED_STRING "9600"
#endif
#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600
#define SERIAL_SPEED_STRING "9600"
+#define SERIAL_SPEED_CHECKSUM "16"
#endif
#endif
-static const char ao_gps_set_nmea[] = "\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*2d\r\n";
+static const char ao_gps_set_nmea[] =
+ "\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*" SERIAL_SPEED_CHECKSUM "\r\n";
struct ao_ublox_cksum {
uint8_t a, b;
struct ao_ublox_cksum {
uint8_t a, b;
@@
-593,6
+601,14
@@
static const uint8_t ublox_enable_nav[] = {
UBLOX_NAV_TIMEUTC
};
UBLOX_NAV_TIMEUTC
};
+void
+ao_gps_set_rate(uint8_t rate)
+{
+ uint8_t i;
+ for (i = 0; i < sizeof (ublox_enable_nav); i++)
+ ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], rate);
+}
+
void
ao_gps(void) __reentrant
{
void
ao_gps(void) __reentrant
{
@@
-609,8
+625,7
@@
ao_gps(void) __reentrant
ao_ublox_set_message_rate(UBLOX_NAV, ublox_disable_nav[i], 0);
/* Enable all of the messages we want */
ao_ublox_set_message_rate(UBLOX_NAV, ublox_disable_nav[i], 0);
/* Enable all of the messages we want */
- for (i = 0; i < sizeof (ublox_enable_nav); i++)
- ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], 1);
+ ao_gps_set_rate(1);
ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE),
UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G,
ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE),
UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G,
@@
-696,9
+711,9
@@
ao_gps(void) __reentrant
continue;
switch (class) {
continue;
switch (class) {
- case
0x01
:
+ case
UBLOX_NAV
:
switch (id) {
switch (id) {
- case
0x21
:
+ case
UBLOX_NAV_TIMEUTC
:
ao_mutex_get(&ao_gps_mutex);
ao_gps_tick = ao_time();
ao_mutex_get(&ao_gps_mutex);
ao_gps_tick = ao_time();
@@
-706,7
+721,7
@@
ao_gps(void) __reentrant
ao_gps_data.flags |= AO_GPS_RUNNING;
if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) {
uint8_t nsat = nav_sol.nsat;
ao_gps_data.flags |= AO_GPS_RUNNING;
if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) {
uint8_t nsat = nav_sol.nsat;
- ao_gps_data.flags |= AO_GPS_VALID;
+ ao_gps_data.flags |= AO_GPS_VALID
| AO_GPS_COURSE_VALID
;
if (nsat > 15)
nsat = 15;
ao_gps_data.flags |= nsat;
if (nsat > 15)
nsat = 15;
ao_gps_data.flags |= nsat;
@@
-714,7
+729,7
@@
ao_gps(void) __reentrant
if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
ao_gps_data.flags |= AO_GPS_DATE_VALID;
if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
ao_gps_data.flags |= AO_GPS_DATE_VALID;
-
ao_gps_data.altitude = nav_posllh.alt_msl / 1000
;
+
AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, nav_posllh.alt_msl / 1000)
;
ao_gps_data.latitude = nav_posllh.lat;
ao_gps_data.longitude = nav_posllh.lon;
ao_gps_data.latitude = nav_posllh.lat;
ao_gps_data.longitude = nav_posllh.lon;
@@
-726,11
+741,11
@@
ao_gps(void) __reentrant
ao_gps_data.minute = nav_timeutc.min;
ao_gps_data.second = nav_timeutc.sec;
ao_gps_data.minute = nav_timeutc.min;
ao_gps_data.second = nav_timeutc.sec;
- ao_gps_data.pdop = nav_dop.pdop;
- ao_gps_data.hdop = nav_dop.hdop;
- ao_gps_data.
vdop = nav_dop.vdop
;
-
- /* mode is not set */
+ /* we report dop scaled by 10, but ublox provides dop scaled by 100
+ */
+ ao_gps_data.
pdop = nav_dop.pdop / 10
;
+ ao_gps_data.hdop = nav_dop.hdop / 10;
+ ao_gps_data.vdop = nav_dop.vdop / 10;
ao_gps_data.ground_speed = nav_velned.g_speed;
ao_gps_data.climb_rate = -nav_velned.vel_d;
ao_gps_data.ground_speed = nav_velned.g_speed;
ao_gps_data.climb_rate = -nav_velned.vel_d;
@@
-739,23
+754,25
@@
ao_gps(void) __reentrant
ao_gps_tracking_data.channels = 0;
struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0];
ao_gps_tracking_data.channels = 0;
struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0];
+ struct nav_svinfo_sat *src = &nav_svinfo_sat[0];
for (i = 0; i < nav_svinfo_nsat; i++) {
for (i = 0; i < nav_svinfo_nsat; i++) {
- struct nav_svinfo_sat *src = &nav_svinfo_sat[i];
-
if (!(src->flags & (1 << NAV_SVINFO_SAT_FLAGS_UNHEALTHY)) &&
src->quality >= NAV_SVINFO_SAT_QUALITY_ACQUIRED)
{
if (!(src->flags & (1 << NAV_SVINFO_SAT_FLAGS_UNHEALTHY)) &&
src->quality >= NAV_SVINFO_SAT_QUALITY_ACQUIRED)
{
- dst->svid = src->svid;
- dst->c_n_1 = src->cno;
- dst++;
- ao_gps_tracking_data.channels++;
+ if (ao_gps_tracking_data.channels < AO_TELEMETRY_SATELLITE_MAX_SAT) {
+ dst->svid = src->svid;
+ dst->c_n_1 = src->cno;
+ dst++;
+ ao_gps_tracking_data.channels++;
+ }
}
}
+ src++;
}
ao_mutex_put(&ao_gps_mutex);
}
ao_mutex_put(&ao_gps_mutex);
- ao_
wakeup(&ao_gps_data)
;
- ao_wakeup(&ao_gps_
tracking_data
);
+ ao_
gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING
;
+ ao_wakeup(&ao_gps_
new
);
break;
}
break;
break;
}
break;