From: Keith Packard Date: Sat, 7 Jun 2014 18:34:29 +0000 (-0700) Subject: altos: Add TeleGPS logging format X-Git-Tag: 1.3.2.3~12 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=302842ccda46a0a3d58b60d5c7fc82e05f614b0b;hp=62aa51f0b785bea146d1e9331e6253de2d547c94 altos: Add TeleGPS logging format This is mostly like the mega format, but places the flight state in a spare byte of the GPS data and writes the gps starting location to the flight packet. Log data is written by the main tracker thread; there's no reason for a separate thread given the GPS update rate and the lack of flight controls. This means ao_log_gps has an API to be called from there, rather than a thread to run. Signed-off-by: Keith Packard --- diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h index 3ff9e811..87fa0d4d 100644 --- a/src/kernel/ao_log.h +++ b/src/kernel/ao_log.h @@ -47,6 +47,7 @@ extern __pdata enum ao_flight_state ao_log_state; #define AO_LOG_FORMAT_EASYMINI 6 /* 16-byte MS5607 baro only, 3.0V supply */ #define AO_LOG_FORMAT_TELEMETRUM 7 /* 16-byte typed telemetrum records */ #define AO_LOG_FORMAT_TELEMINI 8 /* 16-byte MS5607 baro only, 3.3V supply */ +#define AO_LOG_FORMAT_TELEGPS 9 /* 32 byte telegps records */ #define AO_LOG_FORMAT_NONE 127 /* No log at all */ extern __code uint8_t ao_log_format; @@ -364,6 +365,50 @@ struct ao_log_mini { (dst)[2] = (value) >> 16; \ } while (0) +struct ao_log_gps { + char type; /* 0 */ + uint8_t csum; /* 1 */ + uint16_t tick; /* 2 */ + union { /* 4 */ + /* AO_LOG_FLIGHT */ + struct { + uint16_t flight; /* 4 */ + int16_t start_altitude; /* 6 */ + int32_t start_latitude; /* 8 */ + int32_t start_longitude; /* 12 */ + } flight; /* 16 */ + /* AO_LOG_GPS_TIME */ + struct { + int32_t latitude; /* 4 */ + int32_t longitude; /* 8 */ + int16_t altitude; /* 12 */ + uint8_t hour; /* 14 */ + uint8_t minute; /* 15 */ + uint8_t second; /* 16 */ + uint8_t flags; /* 17 */ + uint8_t year; /* 18 */ + uint8_t month; /* 19 */ + uint8_t day; /* 20 */ + uint8_t course; /* 21 */ + uint16_t ground_speed; /* 22 */ + int16_t climb_rate; /* 24 */ + uint8_t pdop; /* 26 */ + uint8_t hdop; /* 27 */ + uint8_t vdop; /* 28 */ + uint8_t mode; /* 29 */ + uint8_t state; /* 30 */ + } gps; /* 31 */ + /* AO_LOG_GPS_SAT */ + struct { + uint16_t channels; /* 4 */ + struct { + uint8_t svid; + uint8_t c_n; + } sats[12]; /* 6 */ + } gps_sat; /* 30 */ + } u; +}; + /* Write a record to the eeprom log */ uint8_t ao_log_data(__xdata struct ao_log_record *log) __reentrant; @@ -377,6 +422,9 @@ ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant; uint8_t ao_log_mini(__xdata struct ao_log_mini *log) __reentrant; +uint8_t +ao_log_gps(__xdata struct ao_log_gps *log) __reentrant; + void ao_log_flush(void); diff --git a/src/kernel/ao_log_gps.c b/src/kernel/ao_log_gps.c new file mode 100644 index 00000000..e9e573a5 --- /dev/null +++ b/src/kernel/ao_log_gps.c @@ -0,0 +1,173 @@ +/* + * Copyright © 2014 Keith Packard + * + * 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include +#include +#include +#include +#include +#include + +static __xdata uint8_t ao_log_mutex; +static __xdata struct ao_log_gps log; + +__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEGPS; + +static uint8_t +ao_log_csum(__xdata uint8_t *b) __reentrant +{ + uint8_t sum = 0x5a; + uint8_t i; + + for (i = 0; i < sizeof (struct ao_log_gps); i++) + sum += *b++; + return -sum; +} + +uint8_t +ao_log_gps(__xdata struct ao_log_gps *log) __reentrant +{ + uint8_t wrote = 0; + /* set checksum */ + log->csum = 0; + log->csum = ao_log_csum((__xdata uint8_t *) log); + ao_mutex_get(&ao_log_mutex); { + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_stop(); + if (ao_log_running) { + wrote = 1; + ao_storage_write(ao_log_current_pos, + log, + sizeof (struct ao_log_gps)); + ao_log_current_pos += sizeof (struct ao_log_gps); + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + + +static int32_t prev_lat, prev_lon; +static int16_t prev_alt; +static uint8_t has_prev, unmoving; + +#define GPS_SPARSE_UNMOVING_REPORTS 10 +#define GPS_SPARSE_UNMOVING_GROUND 10 +#define GPS_SPARSE_UNMOVING_AIR 10 + +uint8_t +ao_log_gps_should_log(int32_t lat, int32_t lon, int16_t alt) +{ + if (has_prev && ao_log_running) { + uint32_t h = ao_distance(prev_lat, prev_lon, lat, lon); + uint16_t v = alt > prev_alt ? (alt - prev_alt) : (prev_alt - alt); + + if (h < GPS_SPARSE_UNMOVING_GROUND && v < GPS_SPARSE_UNMOVING_AIR) { + if (unmoving < GPS_SPARSE_UNMOVING_REPORTS) + ++unmoving; + } else + unmoving = 0; + } else + unmoving = 0; + + prev_lat = lat; + prev_lon = lon; + prev_alt = alt; + has_prev = 1; + return unmoving >= GPS_SPARSE_UNMOVING_REPORTS; +} + +void +ao_log_gps_flight(void) +{ + log.type = AO_LOG_FLIGHT; + log.tick = ao_time(); + log.u.flight.flight = ao_flight_number; + log.u.flight.start_altitude = ao_tracker_start_altitude; + log.u.flight.start_latitude = ao_tracker_start_latitude; + log.u.flight.start_longitude = ao_tracker_start_longitude; + ao_log_gps(&log); +} + +void +ao_log_gps_data(uint16_t tick, uint8_t state, struct ao_telemetry_location *gps_data) +{ + log.tick = tick; + log.type = AO_LOG_GPS_TIME; + log.u.gps.latitude = gps_data->latitude; + log.u.gps.longitude = gps_data->longitude; + log.u.gps.altitude = gps_data->altitude; + + log.u.gps.hour = gps_data->hour; + log.u.gps.minute = gps_data->minute; + log.u.gps.second = gps_data->second; + log.u.gps.flags = gps_data->flags; + log.u.gps.year = gps_data->year; + log.u.gps.month = gps_data->month; + log.u.gps.day = gps_data->day; + log.u.gps.course = gps_data->course; + log.u.gps.ground_speed = gps_data->ground_speed; + log.u.gps.climb_rate = gps_data->climb_rate; + log.u.gps.pdop = gps_data->pdop; + log.u.gps.hdop = gps_data->hdop; + log.u.gps.vdop = gps_data->vdop; + log.u.gps.mode = gps_data->mode; + log.u.gps.state = state; + ao_log_gps(&log); +} + +void +ao_log_gps_tracking(uint16_t tick, struct ao_telemetry_satellite *gps_tracking_data) +{ + uint8_t c, n, i; + + log.tick = tick; + log.type = AO_LOG_GPS_SAT; + i = 0; + n = gps_tracking_data->channels; + for (c = 0; c < n; c++) + if ((log.u.gps_sat.sats[i].svid = gps_tracking_data->sats[c].svid)) + { + log.u.gps_sat.sats[i].c_n = gps_tracking_data->sats[c].c_n_1; + i++; + if (i >= 12) + break; + } + log.u.gps_sat.channels = i; + ao_log_gps(&log); +} + +static uint8_t +ao_log_dump_check_data(void) +{ + if (ao_log_csum((uint8_t *) &log) != 0) + return 0; + return 1; +} + +uint16_t +ao_log_flight(uint8_t slot) +{ + if (!ao_storage_read(ao_log_pos(slot), + &log, + sizeof (struct ao_log_gps))) + return 0; + + if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) + return log.u.flight.flight; + return 0; +} diff --git a/src/kernel/ao_log_gps.h b/src/kernel/ao_log_gps.h new file mode 100644 index 00000000..733db19b --- /dev/null +++ b/src/kernel/ao_log_gps.h @@ -0,0 +1,32 @@ +/* + * Copyright © 2014 Keith Packard + * + * 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include "ao_telemetry.h" + +uint8_t +ao_log_gps_should_log(int32_t lat, int32_t lon, int16_t alt); + +void +ao_log_gps_flight(void); + +void +ao_log_gps_data(uint16_t tick, uint8_t state, struct ao_telemetry_location *gps_data); + +void +ao_log_gps_tracking(uint16_t tick, struct ao_telemetry_satellite *gps_tracking_data); +