*
* 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
#include "ao_gps_ublox.h"
-#define AO_UBLOX_DEBUG 1
+#define AO_UBLOX_DEBUG 0
#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;
+uint8_t ao_gps_new;
+uint8_t ao_gps_mutex;
+uint16_t ao_gps_tick;
+struct ao_telemetry_location ao_gps_data;
+struct ao_telemetry_satellite ao_gps_tracking_data;
#undef AO_SERIAL_SPEED_UBLOX
uint8_t a, b;
};
-static __pdata struct ao_ublox_cksum ao_ublox_cksum;
-static __pdata uint16_t ao_ublox_len;
+static struct ao_ublox_cksum ao_ublox_cksum;
+static uint16_t ao_ublox_len;
#if AO_UBLOX_DEBUG
return header_byte();
}
-static char __xdata *ublox_target;
+static char *ublox_target;
static void ublox_u16(uint8_t offset)
{
- uint16_t __xdata *ptr = (uint16_t __xdata *) (ublox_target + offset);
+ uint16_t *ptr = (uint16_t *) (void *) (ublox_target + offset);
uint16_t val;
val = data_byte();
static void ublox_u8(uint8_t offset)
{
- uint8_t __xdata *ptr = (uint8_t __xdata *) (ublox_target + offset);
+ uint8_t *ptr = (uint8_t *) (ublox_target + offset);
uint8_t val;
val = data_byte ();
*ptr = val;
}
-static void ublox_u32(uint8_t offset) __reentrant
+static void ublox_u32(uint8_t offset)
{
- uint32_t __xdata *ptr = (uint32_t __xdata *) (ublox_target + offset);
+ uint32_t *ptr = (uint32_t *) (void *) (ublox_target + offset);
uint32_t val;
val = ((uint32_t) data_byte ());
};
static void
-ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __reentrant
+ao_ublox_parse(void *target, const struct ublox_packet_parse *parse)
{
uint8_t i, offset;
};
void
-ao_gps(void) __reentrant
+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)
{
uint8_t class, id;
struct ao_ublox_cksum cksum;
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_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 (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.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_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++) {
- 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)
{
- 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);
#if AO_UBLOX_DEBUG
static void ao_gps_option(void)
{
- ao_cmd_hex();
+ uint16_t r = ao_cmd_hex();
if (ao_cmd_status != ao_cmd_success) {
ao_cmd_status = ao_cmd_success;
ao_gps_show();
} else {
- ao_gps_dbg_enable = ao_cmd_lex_i;
+ ao_gps_dbg_enable = r;
printf ("gps debug set to %d\n", ao_gps_dbg_enable);
}
}
#define ao_gps_option ao_gps_show
#endif
-__code struct ao_cmds ao_gps_cmds[] = {
+const struct ao_cmds ao_gps_cmds[] = {
{ ao_gps_option, "g\0Display GPS" },
{ 0, NULL },
};
-__xdata struct ao_task ao_gps_task;
+struct ao_task ao_gps_task;
void
ao_gps_init(void)
projects / fw / altos / blobdiff