From: Keith Packard Date: Sun, 28 Apr 2024 05:36:20 +0000 (-0700) Subject: drivers: Add Mosaic GPS driver support X-Git-Url: https://git.gag.com/?a=commitdiff_plain;h=ba8dffff46c73df511649b5292616a46dbb406b2;p=fw%2Faltos drivers: Add Mosaic GPS driver support This allows switching between on-board and external Mosaic GPS receivers. Signed-off-by: Keith Packard --- diff --git a/src/drivers/ao_companion.c b/src/drivers/ao_companion.c index bda7805f..d91a5a83 100644 --- a/src/drivers/ao_companion.c +++ b/src/drivers/ao_companion.c @@ -95,26 +95,6 @@ ao_companion_notify(void) COMPANION_DESELECT(); } -static void -ao_companion(void) -{ - uint8_t i; - while (!ao_flight_number) - ao_sleep(&ao_flight_number); - for (i = 0; i < 10; i++) { - ao_delay(AO_SEC_TO_TICKS(1)); - if ((ao_companion_running = ao_companion_get_setup())) - break; - } - while (ao_companion_running) { - if (ao_sleep_for(&ao_flight_state, ao_companion_setup.update_period)) - ao_companion_get_data(); - else - ao_companion_notify(); - } - ao_exit(); -} - static void ao_companion_status(void) { @@ -139,12 +119,37 @@ const struct ao_cmds ao_companion_cmds[] = { { 0, NULL }, }; +static void +ao_companion(void) +{ + uint8_t i; +#if HAS_GPS_MOSAIC + ao_config_get(); + if (ao_config.gps_mosaic) + ao_exit(); +#endif + ao_enable_output(AO_COMPANION_CS_PORT, AO_COMPANION_CS_PIN, 1); + ao_cmd_register(&ao_companion_cmds[0]); + while (!ao_flight_number) + ao_sleep(&ao_flight_number); + for (i = 0; i < 10; i++) { + ao_delay(AO_SEC_TO_TICKS(1)); + if ((ao_companion_running = ao_companion_get_setup())) + break; + } + while (ao_companion_running) { + if (ao_sleep_for(&ao_flight_state, ao_companion_setup.update_period)) + ao_companion_get_data(); + else + ao_companion_notify(); + } + ao_exit(); +} + static struct ao_task ao_companion_task; void ao_companion_init(void) { - ao_enable_output(AO_COMPANION_CS_PORT, AO_COMPANION_CS_PIN, 1); - ao_cmd_register(&ao_companion_cmds[0]); ao_add_task(&ao_companion_task, ao_companion, "companion"); } diff --git a/src/drivers/ao_gps_mosaic.c b/src/drivers/ao_gps_mosaic.c new file mode 100644 index 00000000..874acfda --- /dev/null +++ b/src/drivers/ao_gps_mosaic.c @@ -0,0 +1,360 @@ +/* + * Copyright © 2024 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, 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 + * 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. + */ + +#ifndef AO_GPS_TEST +#include "ao.h" +#endif + +#if HAS_GPS_MOSAIC + +#include "ao_gps_mosaic.h" +#include "ao_crc_ccitt.h" +#include +#include +#include + +#define AO_MOSAIC_DEBUG 0 + +#if AO_MOSAIC_DEBUG + +#define DBG_PROTO 1 +#define DBG_CHAR 2 +#define DBG_INIT 4 + +static uint8_t mosaic_dbg_enable = 0; + +static void mosaic_dbg(int level, char *format, ...) { + va_list a; + + if (level & mosaic_dbg_enable) { + va_start(a, format); + vprintf(format, a); + va_end(a); + flush(); + } +} + +#else +#define mosaic_dbg(fmt, ...) +#endif + +static inline uint8_t mosaic_byte(void) { + uint8_t c = (uint8_t) ao_mosaic_getchar(); +#if AO_MOSAIC_DEBUG + if (' ' <= c && c <= '~') + mosaic_dbg(DBG_CHAR, "%c", c); + else + mosaic_dbg(DBG_CHAR, " %02x", c); +#endif + return c; +} + +static inline void mosaic_putc(char c) { +#if AO_MOSAIC_DEBUG + if (' ' <= c && c <= '~') + mosaic_dbg(DBG_CHAR, "%c", c); + else + mosaic_dbg(DBG_CHAR, " (%02x)", c); +#endif + ao_mosaic_putchar(c); +} + +static void mosaic_puts(const char *s) { + char c; + while ((c = *s++) != '\0') + mosaic_putc(c); +} + +extern uint8_t ao_gps_new; +extern uint8_t ao_gps_mutex; +extern AO_TICK_TYPE ao_gps_tick; +extern AO_TICK_TYPE ao_gps_utc_tick; +extern struct ao_telemetry_location ao_gps_data; +extern struct ao_telemetry_satellite ao_gps_tracking_data; + +static void +mosaic_wait_idle(void) +{ + uint8_t b; + for (;;) { + while(mosaic_byte() != '$') + ; + if (mosaic_byte() != 'R') + continue; + for (;;) { + b = mosaic_byte(); + if (b == '\n') + return; + } + } +} + +static void +mosaic_command(const char *cmd) +{ + mosaic_puts(cmd); + mosaic_wait_idle(); +} + +static void +mosaic_setup(void) +{ + ao_mosaic_set_speed(AO_SERIAL_SPEED_115200); + /* Send messages always */ + mosaic_command("smrf, OnlyRef \r"); + /* Set receiver dynamics mode */ + mosaic_command("srd, High, Unlimited \r"); + /* Report position once per second */ + /* Report time once per second */ + /* Report sat info once per second */ + mosaic_command("sso, Stream1, COM3, ReceiverTime+MeasEpoch+PVTGeodetic+DOP, sec1 \r"); +} + +#if AO_MOSAIC_DEBUG && !defined(AO_GPS_TEST) +static void mosaic_option(void) +{ + uint8_t r = (uint8_t) ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) { + ao_cmd_status = ao_cmd_success; + ao_gps_show(); + } else { + mosaic_dbg_enable = r; + printf ("gps debug set to %d\n", mosaic_dbg_enable); + } +} +#else +#define mosaic_option ao_gps_show +#endif + +const struct ao_cmds ao_mosaic_cmds[] = { + { mosaic_option, "g\0Display Mosaic GPS" }, + { 0, NULL }, +}; + +static struct sbf sbf; + +static void +mosaic_read(void *buf, size_t len) +{ + uint8_t *b = buf; + while (len--) + *b++ = mosaic_byte(); +} + +static uint16_t +mosaic_read_crc(void *buf, size_t len, uint16_t crc) +{ + uint8_t *b = buf; + + while (len--) { + uint8_t byte = mosaic_byte(); + crc = ao_crc16_ccitt(crc, byte); + *b++ = byte; + } + return crc; +} + +static struct ao_telemetry_location new_location; +static struct ao_telemetry_satellite new_satellite; + +static +int32_t +clip_value(float value, float low, float high) +{ + if (value < low) + value = low; + else if (value > high) + value = high; + return (int32_t) roundf(value); +} + +void +ao_gps_mosaic(void) +{ + AO_TICK_TYPE packet_start_tick; + AO_TICK_TYPE solution_tick = 0; + +#ifndef AO_GPS_TEST + ao_cmd_register(&ao_mosaic_cmds[0]); +#endif + mosaic_setup(); + + for (;;) { + uint16_t crc_computed; + + while(mosaic_byte() != '$') + ; + if (mosaic_byte() != '@') + continue; + packet_start_tick = ao_tick_count; + + mosaic_read(&sbf.header.crc, sizeof(sbf.header.crc)); + crc_computed = mosaic_read_crc(&sbf.header.h, sizeof(sbf.header.h), 0); + if (sbf.header.h.length > sizeof(sbf) + 2) { + mosaic_dbg(DBG_PROTO, "too long %d > %ld\n", + sbf.header.h.length, sizeof(sbf) + 2); + continue; + } + crc_computed = mosaic_read_crc(sbf.data, sbf.header.h.length - 8, crc_computed); + if (crc_computed != sbf.header.crc) { + mosaic_dbg(DBG_PROTO, "crc error (computed 0x%04x)\n", crc_computed); + continue; + } + + bool gps_ready = false; + + switch (SBF_BLOCK_NUMBER(sbf.header.h.id)) { + case SBF_RECEIVER_TIME: + solution_tick = packet_start_tick; + memset(&new_location, 0, sizeof(new_location)); + memset(&new_satellite, 0, sizeof(new_satellite)); + new_location.year = (uint8_t) sbf.receiver_time.utcyear; + new_location.month = (uint8_t) sbf.receiver_time.utcmonth; + new_location.day = (uint8_t) sbf.receiver_time.utcday; + new_location.hour = (uint8_t) sbf.receiver_time.utchour; + new_location.minute = (uint8_t) sbf.receiver_time.utcmin; + new_location.second = (uint8_t) sbf.receiver_time.utcsec; + + if (sbf.receiver_time.utcyear != -128 && + sbf.receiver_time.utcmonth != -128 && + sbf.receiver_time.utcday != -128 && + sbf.receiver_time.utchour != -128 && + sbf.receiver_time.utcmin != -128 && + sbf.receiver_time.utcsec != -128) + { + new_location.flags |= AO_GPS_DATE_VALID; + } + + mosaic_dbg(DBG_PROTO, "ReceiverTime year %d month %d day %d hour %d min %d sec %d\n", + sbf.receiver_time.utcyear, + sbf.receiver_time.utcmonth, + sbf.receiver_time.utcday, + sbf.receiver_time.utchour, + sbf.receiver_time.utcmin, + sbf.receiver_time.utcsec); + break; + case SBF_MEAS_EPOCH: + mosaic_dbg(DBG_PROTO, "MeasEpoch sb1len %d sb2len %d\n", + sbf.meas_epoch.sb1length, + sbf.meas_epoch.sb2length); + { + uint8_t i1, i2; + uint8_t nsat = 0, nsol = 0; + struct sbf_meas_epoch_channel_type1 *type1; + struct sbf_meas_epoch_channel_type2 *type2; + + type1 = (void *) (&sbf.meas_epoch + 1); + for (i1 = 0; i1 < sbf.meas_epoch.n1; i1++) { + uint8_t signal_type = type1->type & 0x1f; + uint8_t cn0; + + if (signal_type == 1 || signal_type == 2) + cn0 = type1->cn0 / 4; + else + cn0 = type1->cn0 / 4 + 10; + if (nsat < AO_TELEMETRY_SATELLITE_MAX_SAT) { + new_satellite.sats[nsat].svid = type1->svid; + new_satellite.sats[nsat].c_n_1 = cn0; + new_satellite.channels = nsat + 1; + } + nsat++; + if (type1->locktime != 65535) + nsol++; + mosaic_dbg(DBG_PROTO, " Type1 type 0x%x channel %d svid %d cn0 %d locktime %u\n", + type1->type, type1->rxchannel, type1->svid, cn0, type1->locktime); + type2 = (void *) ((uint8_t *) type1 + sbf.meas_epoch.sb1length); + for (i2 = 0; i2 < type1->n2; i2++) { + mosaic_dbg(DBG_PROTO, " Type2 type %d cn0 %d\n", + type2->type, type2->cn0); + type2 = (void *) ((uint8_t *) type2 + sbf.meas_epoch.sb1length); + } + type1 = (void *) type2; + } + if (nsol > 15) + nsol = 15; + new_location.flags |= nsol; + } + break; + case SBF_PVT_GEODETIC: + mosaic_dbg(DBG_PROTO, "PVTGeodetic mode 0x%02x error %d lat %f lon %f height %f\n", + sbf.geodetic.mode, + sbf.geodetic.error, + sbf.geodetic.latitude * 180.0/M_PI, + sbf.geodetic.longitude * 180.0/M_PI, + sbf.geodetic.height); + + /* Need to use double here to preserve all of the available precision */ + new_location.latitude = (int32_t) round(sbf.geodetic.latitude * (1e7 * 180.0/M_PI)); + new_location.longitude = (int32_t) round(sbf.geodetic.longitude * (1e7 * 180.0f/M_PI)); + gps_alt_t altitude = (gps_alt_t) round(sbf.geodetic.height); + AO_TELEMETRY_LOCATION_SET_ALTITUDE(&new_location, altitude); + if (sbf.geodetic.latitude != -2e10 && + sbf.geodetic.longitude != -2e10 && + sbf.geodetic.height != -2e10) + { + new_location.flags |= AO_GPS_VALID; + } + if (sbf.geodetic.vn != -2e10 && + sbf.geodetic.ve != -2e10 && + sbf.geodetic.vu != -2e10) + { + new_location.flags |= AO_GPS_COURSE_VALID; + } + float ground_speed = hypotf((float) sbf.geodetic.vn, (float) sbf.geodetic.ve); + float course = atan2f((float) sbf.geodetic.ve, (float) sbf.geodetic.vn); + new_location.ground_speed = (uint16_t) clip_value(ground_speed, 0, 65535.0f); + new_location.climb_rate = (int16_t) clip_value(sbf.geodetic.vu * 100, -32768.0f, 32767.0f); + new_location.course = (uint8_t) clip_value(course * (90.0f/(float)M_PI), 0.0f, 255.0f); + break; + case SBF_DOP: + mosaic_dbg(DBG_PROTO, "DOP pdop%d tdop %d hdop %d vdop %d\n", + sbf.dop.pdop, + sbf.dop.tdop, + sbf.dop.hdop, + sbf.dop.vdop); + new_location.pdop = (uint8_t) (sbf.dop.pdop / 10); + new_location.hdop = (uint8_t) (sbf.dop.hdop / 10); + new_location.vdop = (uint8_t) (sbf.dop.vdop / 10); + gps_ready = true; + break; + default: + mosaic_dbg(DBG_PROTO, "block %d revision %d length %d avail %ld\n", + SBF_BLOCK_NUMBER(sbf.header.h.id), + SBF_BLOCK_REVISION(sbf.header.h.id), + sbf.header.h.length, + sizeof(sbf)); + break; + } + + if (!gps_ready) + continue; + + new_location.flags |= AO_GPS_RUNNING; + + ao_mutex_get(&ao_gps_mutex); + ao_gps_data = new_location; + ao_gps_tracking_data = new_satellite; + ao_gps_tick = solution_tick; + + ao_mutex_put(&ao_gps_mutex); + ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING; + ao_wakeup(&ao_gps_new); + } +} + +#endif diff --git a/src/drivers/ao_gps_mosaic.h b/src/drivers/ao_gps_mosaic.h new file mode 100644 index 00000000..88ab03a2 --- /dev/null +++ b/src/drivers/ao_gps_mosaic.h @@ -0,0 +1,152 @@ +/* + * Copyright © 2024 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, 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 + * 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. + */ + +#ifndef _AO_GPS_MOSAIC_H_ +#define _AO_GPS_MOSAIC_H_ + +void +ao_gps_mosaic(void); + +struct sbf_header { + uint16_t crc; + struct { + uint16_t id; + uint16_t length; + } h; +}; + +#define SBF_BLOCK_NUMBER(id) ((id) & 0x1fff) +#define SBF_BLOCK_REVISION(id) ((id) >> 13) + +#define SBF_MEAS_EPOCH 4027 + +struct sbf_meas_epoch { + uint32_t tow; + uint16_t wnc; + uint8_t n1; + uint8_t sb1length; + uint8_t sb2length; + uint8_t commonflags; + uint8_t cumclkjumps; + uint8_t reserved; +}; + +struct sbf_meas_epoch_channel_type1 { + uint8_t rxchannel; + uint8_t type; + uint8_t svid; + uint8_t misc; + uint32_t codelsb; + int32_t doppler; + uint16_t carrierlsb; + int8_t carriermsb; + uint8_t cn0; + uint16_t locktime; + uint8_t obsinfo; + uint8_t n2; +}; + +struct sbf_meas_epoch_channel_type2 { + uint8_t type; + uint8_t locktime; + uint8_t cn0; + uint8_t offsetmsb; + int8_t carriermsb; + uint8_t obsinfo; + uint16_t codeoffsetlsb; + uint16_t carrierlsb; + uint16_t doppleroffsetlsb; +}; + +#define SBF_DOP 4001 + +struct sbf_dop { + uint32_t tow; + uint16_t wnc; + uint8_t nrsv; + uint8_t reserved; + uint16_t pdop; + uint16_t tdop; + uint16_t hdop; + uint16_t vdop; + float hpl; + float vpl; +}; + +#define SBF_PVT_GEODETIC 4007 + +struct sbf_pvt_geodetic { + uint32_t tow; + uint16_t wnc; + uint8_t mode; + uint8_t error; + double latitude; + double longitude; + double height; + float undulation; + float vn; + float ve; + float vu; + float cog; + double rxclkbias; + float rxclkdrift; + uint8_t timesystem; + uint8_t datum; + uint8_t nrsv; + uint8_t wacorrinfo; + uint16_t referenceid; + uint16_t meancorrage; + uint32_t signalinfo; + uint8_t alertflag; + uint8_t nrbases; + uint16_t pppinfo; + uint16_t latency; + uint16_t haccuracy; + uint16_t vaccuracy; + uint8_t misc; +}; + +#define SBF_RECEIVER_TIME 5914 + +struct sbf_receiver_time { + uint32_t tow; + uint16_t wnc; + int8_t utcyear; + int8_t utcmonth; + int8_t utcday; + int8_t utchour; + int8_t utcmin; + int8_t utcsec; + int8_t deltals; + uint8_t synclevel; +}; + +#define MAX_SBF_DATA 1024 + +struct sbf { + struct sbf_header header; + union { + struct sbf_receiver_time receiver_time; + struct sbf_meas_epoch meas_epoch; + struct sbf_pvt_geodetic geodetic; + struct sbf_dop dop; + uint8_t data[MAX_SBF_DATA]; + }; +}; + +#endif /* _AO_GPS_MOSAIC_H_ */ diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 15a74a6e..9afa5d38 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -20,6 +20,10 @@ #include "ao.h" #endif +#ifdef HAS_GPS_MOSAIC +#include "ao_gps_mosaic.h" +#endif + #include "ao_gps_ublox.h" #define AO_UBLOX_DEBUG 0 @@ -819,6 +823,27 @@ ao_gps_set_rate(uint8_t rate) ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], rate); } +#if AO_UBLOX_DEBUG +static void ao_gps_option(void) +{ + uint8_t r = (uint8_t) ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) { + ao_cmd_status = ao_cmd_success; + ao_gps_show(); + } else { + ao_gps_dbg_enable = r; + printf ("gps debug set to %d\n", ao_gps_dbg_enable); + } +} +#else +#define ao_gps_option ao_gps_show +#endif + +const struct ao_cmds ao_gps_cmds[] = { + { ao_gps_option, "g\0Display GPS" }, + { 0, NULL }, +}; + void ao_gps(void) { @@ -828,6 +853,17 @@ ao_gps(void) AO_TICK_TYPE packet_start_tick; AO_TICK_TYPE solution_tick = 0; +#ifdef HAS_GPS_MOSAIC + ao_config_get(); + if (ao_config.gps_mosaic) { + ao_gps_mosaic(); + return; + } +#endif +#ifndef AO_GPS_TEST + ao_cmd_register(&ao_gps_cmds[0]); +#endif + ao_gps_setup(); /* Disable all messages */ @@ -1067,32 +1103,10 @@ ao_gps(void) } } -#if AO_UBLOX_DEBUG -static void ao_gps_option(void) -{ - uint8_t r = (uint8_t) ao_cmd_hex(); - if (ao_cmd_status != ao_cmd_success) { - ao_cmd_status = ao_cmd_success; - ao_gps_show(); - } else { - ao_gps_dbg_enable = r; - printf ("gps debug set to %d\n", ao_gps_dbg_enable); - } -} -#else -#define ao_gps_option ao_gps_show -#endif - -const struct ao_cmds ao_gps_cmds[] = { - { ao_gps_option, "g\0Display GPS" }, - { 0, NULL }, -}; - struct ao_task ao_gps_task; void ao_gps_init(void) { - ao_cmd_register(&ao_gps_cmds[0]); ao_add_task(&ao_gps_task, ao_gps, "gps"); } diff --git a/src/kernel/ao.h b/src/kernel/ao.h index b3f780f5..9b81fe5c 100644 --- a/src/kernel/ao.h +++ b/src/kernel/ao.h @@ -827,7 +827,13 @@ extern const char ao_product[]; * Fifos */ +#ifndef AO_FIFO_SIZE +#ifdef HAS_GPS_MOSAIC +#define AO_FIFO_SIZE 256 +#else #define AO_FIFO_SIZE 32 +#endif +#endif struct ao_fifo { uint8_t insert; diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c index 565ac61e..bd4aa4ee 100644 --- a/src/kernel/ao_config.c +++ b/src/kernel/ao_config.c @@ -254,6 +254,10 @@ _ao_config_get(void) #endif if (minor < 26) ao_config.report_feet = AO_CONFIG_DEFAULT_REPORT_FEET; +#if HAS_GPS_MOSAIC + if (minor < 27) + ao_config.gps_mosaic = 0; +#endif ao_config.minor = AO_CONFIG_MINOR; ao_config_dirty = 1; } @@ -902,6 +906,25 @@ ao_config_report_feet_set(void) } #endif +#if HAS_GPS_MOSAIC +static void +ao_config_gps_mosaic_show(void) +{ + printf ("GPS receiver: %d\n", ao_config.gps_mosaic); +} + +static void +ao_config_gps_mosaic_set(void) +{ + uint32_t r = ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.gps_mosaic = !!r; + _ao_config_edit_finish(); +} +#endif + #if HAS_BEEP static void ao_config_beep_show(void) @@ -1156,6 +1179,10 @@ const struct ao_config_var ao_config_vars[] = { #if HAS_BARO { "u <0 meters, 1 feet>\0Units to report height after landing", ao_config_report_feet_set, ao_config_report_feet_show }, +#endif +#if HAS_GPS_MOSAIC + { "g <0 ublox, 1 mosaic>\0Select GPS receiver", + ao_config_gps_mosaic_set, ao_config_gps_mosaic_show }, #endif { "s\0Show", ao_config_show, 0 }, diff --git a/src/kernel/ao_config.h b/src/kernel/ao_config.h index a8541775..77b1494b 100644 --- a/src/kernel/ao_config.h +++ b/src/kernel/ao_config.h @@ -62,7 +62,7 @@ #endif #define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 26 +#define AO_CONFIG_MINOR 27 /* All cc1200 devices support limiting TX power to 10mW */ #if !defined(HAS_RADIO_10MW) && defined(AO_CC1200_SPI) @@ -139,6 +139,9 @@ struct ao_config { uint8_t radio_10mw; /* minor version 25 */ #endif uint8_t report_feet; /* minor version 26 */ +#if HAS_GPS_MOSAIC + uint8_t gps_mosaic; /* minor version 27 */ +#endif }; struct ao_config_1_24 { diff --git a/src/kernel/ao_crc_ccitt.c b/src/kernel/ao_crc_ccitt.c new file mode 100644 index 00000000..946402b2 --- /dev/null +++ b/src/kernel/ao_crc_ccitt.c @@ -0,0 +1,54 @@ +/* + * Copyright © 2024 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, 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 + * 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_crc_ccitt.h" + +const uint16_t ao_crc16_ccitt_table[256] = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, + 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, + 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, + 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, + 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, + 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, + 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, + 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, + 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, + 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, + 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, + 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, + 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, + 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, + 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, + 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, + 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, + 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, + 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, + 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, + 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, + 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, + 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, + 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, + 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0 +}; diff --git a/src/kernel/ao_crc_ccitt.h b/src/kernel/ao_crc_ccitt.h new file mode 100644 index 00000000..7393dcda --- /dev/null +++ b/src/kernel/ao_crc_ccitt.h @@ -0,0 +1,33 @@ +/* + * Copyright © 2024 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, 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 + * 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. + */ + +#ifndef _AO_CRC_CCITT_H_ +#define _AO_CRC_CCITT_H_ + +#include + +extern const uint16_t ao_crc16_ccitt_table[256]; + +static inline uint16_t ao_crc16_ccitt(uint16_t crc, uint8_t byte) +{ + uint16_t high = crc << 8; + uint8_t low = (uint8_t) (crc >> 8) ^ byte; + return high ^ ao_crc16_ccitt_table[low]; +} + +#endif /* _AO_CRC_CCITT_H_ */ diff --git a/src/kernel/ao_host.h b/src/kernel/ao_host.h index c974a9fe..c3a4fa51 100644 --- a/src/kernel/ao_host.h +++ b/src/kernel/ao_host.h @@ -23,7 +23,9 @@ #include #include #include +#include +#define AO_TICK_TYPE uint32_t #define AO_ADC_RING 64 #define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1)) #define ao_adc_ring_prev(n) (((n) - 1) & (AO_ADC_RING - 1)) @@ -56,6 +58,7 @@ enum ao_flight_state { struct ao_adc ao_adc_ring[AO_ADC_RING]; uint8_t ao_adc_head; +AO_TICK_TYPE ao_tick_count; #define ao_led_on(l) #define ao_led_off(l) @@ -86,8 +89,12 @@ struct ao_task { #define ao_log_start() #define ao_log_stop() -#define AO_MS_TO_TICKS(ms) ((ms) / 10) -#define AO_SEC_TO_TICKS(s) ((s) * 100) +#ifndef AO_HERTZ +#define AO_HERTZ 100 +#endif +#define AO_MS_TO_TICKS(ms) ((ms) / (1000 / AO_HERTZ)) +#define AO_SEC_TO_TICKS(s) ((AO_TICK_TYPE) (s) * AO_HERTZ) +#define AO_NS_TO_TICKS(ns) ((ns) / (1000000000L / AO_HERTZ)) #define AO_FLIGHT_TEST @@ -115,6 +122,9 @@ struct ao_cmds { struct ao_config { uint16_t main_deploy; int16_t accel_zero_g; +#ifdef HAS_MOSAIC + uint8_t gps_mosaic; +#endif }; #define ao_config_get() @@ -124,3 +134,5 @@ struct ao_config ao_config = { 250, 16000 }; #define memcpy(d,s,c) memcpy(d,s,c) #define memset(d,v,c) memset(d,v,c) #define memcmp(d,s,c) memcmp(d,s,c) + +#define flush() fflush(stdout) diff --git a/src/test/.gitignore b/src/test/.gitignore index a7cfad72..8890cc8e 100644 --- a/src/test/.gitignore +++ b/src/test/.gitignore @@ -6,6 +6,7 @@ ao_flight_test_accel ao_gps_test ao_gps_test_skytraq ao_gps_test_ublox +ao_gps_test_mosaic ao_int64_test ao_ms5607_convert_test ao_quaternion_test diff --git a/src/test/Makefile b/src/test/Makefile index 55a3fbeb..63922b5b 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -18,7 +18,8 @@ KALMAN=make-kalman CFLAGS=-I.. -I. -I../kernel -I../drivers -I../micropeak -I../product -I../lisp -O0 -g -Wall -DAO_LISP_TEST -no-pie -all: $(PROGS) ao_aprs_data.wav +#all: $(PROGS) ao_aprs_data.wav +all: ao_gps_test_mosaic clean:: rm -f $(PROGS) ao_aprs_data.wav run-out.baro run-out.full @@ -58,6 +59,9 @@ ao_gps_test_skytraq: ao_gps_test_skytraq.c ao_gps_skytraq.c ao_gps_print.c ao_gp ao_gps_test_ublox: ao_gps_test_ublox.c ao_gps_ublox.c ao_gps_print.c ao_gps_show.c ao_host.h ao_gps_ublox.h cc $(CFLAGS) -o $@ $< +ao_gps_test_mosaic: ao_gps_test_mosaic.c ao_gps_mosaic.c ao_gps_print.c ao_gps_show.c ao_host.h ao_gps_mosaic.h + cc $(CFLAGS) -o $@ $< -lm + ao_convert_test: ao_convert_test.c ao_convert.c altitude.h cc $(CFLAGS) -o $@ $< diff --git a/src/test/ao_gps_test_mosaic.c b/src/test/ao_gps_test_mosaic.c new file mode 100644 index 00000000..cbc29d0e --- /dev/null +++ b/src/test/ao_gps_test_mosaic.c @@ -0,0 +1,281 @@ +/* + * Copyright © 2024 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, 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 + * 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. + */ + +#define AO_GPS_TEST +#define HAS_GPS 1 +#define HAS_GPS_MOSAIC 1 +#include "ao_host.h" +#include +#include +#include +#include +#include +#include + +#define AO_GPS_NUM_SAT_MASK (0xf << 0) +#define AO_GPS_NUM_SAT_SHIFT (0) + +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + +#define AO_GPS_VALID (1 << 4) +#define AO_GPS_RUNNING (1 << 5) +#define AO_GPS_DATE_VALID (1 << 6) +#define AO_GPS_COURSE_VALID (1 << 7) + +struct ao_telemetry_location { + uint8_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint8_t second; + uint8_t flags; + int32_t latitude; /* degrees * 10⁷ */ + int32_t longitude; /* degrees * 10⁷ */ + int16_t altitude_low; /* m */ + uint16_t ground_speed; /* cm/s */ + uint8_t course; /* degrees / 2 */ + uint8_t pdop; /* * 5 */ + uint8_t hdop; /* * 5 */ + uint8_t vdop; /* * 5 */ + int16_t climb_rate; /* cm/s */ + uint16_t h_error; /* m */ + uint16_t v_error; /* m */ + int16_t altitude_high; /* m */ +}; + +typedef int32_t gps_alt_t; +#define AO_TELEMETRY_LOCATION_ALTITUDE(l) (((gps_alt_t) (l)->altitude_high << 16) | ((l)->altitude_low)) +#define AO_GPS_ORIG_ALTITUDE(l) AO_TELEMETRY_LOCATION_ALTITUDE(l) +#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) (((l)->altitude_high = (a) >> 16), \ + ((l)->altitude_low = (a))) + +#define UBLOX_SAT_STATE_ACQUIRED (1 << 0) +#define UBLOX_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) +#define UBLOX_SAT_BIT_SYNC_COMPLETE (1 << 2) +#define UBLOX_SAT_SUBFRAME_SYNC_COMPLETE (1 << 3) +#define UBLOX_SAT_CARRIER_PULLIN_COMPLETE (1 << 4) +#define UBLOX_SAT_CODE_LOCKED (1 << 5) +#define UBLOX_SAT_ACQUISITION_FAILED (1 << 6) +#define UBLOX_SAT_EPHEMERIS_AVAILABLE (1 << 7) + +struct ao_telemetry_satellite_info { + uint8_t svid; + uint8_t c_n_1; +}; + +#define AO_TELEMETRY_SATELLITE_MAX_SAT 12 + +struct ao_telemetry_satellite { + uint8_t channels; + struct ao_telemetry_satellite_info sats[AO_TELEMETRY_SATELLITE_MAX_SAT]; +}; + +#define ao_gps_orig ao_telemetry_location +#define ao_gps_tracking_orig ao_telemetry_satellite +#define ao_gps_sat_orig ao_telemetry_satellite_info + +uint8_t ao_gps_new; +uint8_t ao_gps_mutex; +AO_TICK_TYPE ao_gps_tick; +AO_TICK_TYPE ao_gps_utc_tick; +struct ao_telemetry_location ao_gps_data; +struct ao_telemetry_satellite ao_gps_tracking_data; + +void +ao_mutex_get(uint8_t *mutex) +{ +} + +void +ao_mutex_put(uint8_t *mutex) +{ +} + +static int ao_gps_fd; +static FILE *ao_gps_file; + +#if 0 +static void +ao_dbg_char(char c) +{ + char line[128]; + line[0] = '\0'; + if (c < ' ') { + if (c == '\n') + sprintf (line, "\n"); + else + sprintf (line, "\\%02x", ((int) c) & 0xff); + } else { + sprintf (line, "%c", c); + } + write(1, line, strlen(line)); +} +#endif + +#include + +int +get_millis(void) +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return tv.tv_sec * 1000 + tv.tv_usec / 1000; +} + +char +ao_mosaic_getchar(void) +{ + char c; + int i; + + i = getc(ao_gps_file); + if (i == EOF) { + perror("getchar"); + exit(1); + } + c = i; + return c; +} + +void +ao_mosaic_putchar(char c) +{ + int i; + + for (;;) { + i = write(ao_gps_fd, &c, 1); + if (i == 1) + break; + if (i < 0 && (errno == EINTR || errno == EAGAIN)) + continue; + perror("putchar"); + exit(1); + } +} + +#define AO_SERIAL_SPEED_4800 0 +#define AO_SERIAL_SPEED_9600 1 +#define AO_SERIAL_SPEED_57600 2 +#define AO_SERIAL_SPEED_115200 3 + +static void +ao_gps_set_speed(uint8_t speed) +{ + int fd = ao_gps_fd; + struct termios termios; + + printf ("\t\tset speed %d\n", speed); + tcdrain(fd); + tcgetattr(fd, &termios); + switch (speed) { + case AO_SERIAL_SPEED_4800: + cfsetspeed(&termios, B4800); + break; + case AO_SERIAL_SPEED_9600: + cfsetspeed(&termios, B9600); + break; + case AO_SERIAL_SPEED_57600: + cfsetspeed(&termios, B57600); + break; + case AO_SERIAL_SPEED_115200: + cfsetspeed(&termios, B115200); + break; + } + tcsetattr(fd, TCSAFLUSH, &termios); + tcflush(fd, TCIFLUSH); +} + +#define ao_time() 0 + +uint8_t ao_task_minimize_latency; + +#define ao_usb_getchar() 0 + +#include "ao_gps_print.c" +#include "ao_gps_show.c" +#include "ao_gps_mosaic.c" +#include "ao_crc_ccitt.c" + +void +ao_dump_state(void *wchan) +{ + if (wchan == &ao_gps_new) + ao_gps_show(); + return; +} + +int +ao_gps_open(const char *tty) +{ + struct termios termios; + int fd; + + fd = open (tty, O_RDWR); + if (fd < 0) + return -1; + + tcgetattr(fd, &termios); + cfmakeraw(&termios); + cfsetspeed(&termios, B4800); + tcsetattr(fd, TCSAFLUSH, &termios); + + tcdrain(fd); + tcflush(fd, TCIFLUSH); + return fd; +} + +#include + +static const struct option options[] = { + { .name = "tty", .has_arg = 1, .val = 'T' }, + { 0, 0, 0, 0}, +}; + +static void usage(char *program) +{ + fprintf(stderr, "usage: %s [--tty ]\n", program); + exit(1); +} + +int +main (int argc, char **argv) +{ + char *tty = "/dev/ttyUSB0"; + int c; + + while ((c = getopt_long(argc, argv, "T:", options, NULL)) != -1) { + switch (c) { + case 'T': + tty = optarg; + break; + default: + usage(argv[0]); + break; + } + } + ao_gps_fd = ao_gps_open(tty); + if (ao_gps_fd < 0) { + perror (tty); + exit (1); + } + ao_gps_file = fdopen(ao_gps_fd, "r"); + mosaic(); + return 0; +} diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c index 0833e4f6..03494670 100644 --- a/src/test/ao_gps_test_ublox.c +++ b/src/test/ao_gps_test_ublox.c @@ -25,6 +25,7 @@ #include #include #include + #define AO_GPS_NUM_SAT_MASK (0xf << 0) #define AO_GPS_NUM_SAT_SHIFT (0)