From 4a06dab7b540c20c410e454827e13bb1fbd4b7a6 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Wed, 4 Mar 2015 21:30:20 +0100 Subject: [PATCH] gps: split gps_venus Add a new generic interface, prepare for addition of gps_ubx --- imuboard/Makefile | 1 + imuboard/commands.c | 2 +- imuboard/gps.c | 110 ++++++++++++++++++++++++++++++++++++++++ imuboard/gps.h | 59 +++++++++++++++++++++ imuboard/gps_venus.c | 96 +---------------------------------- imuboard/gps_venus.h | 48 ------------------ imuboard/i2c_protocol.c | 2 +- imuboard/main.c | 1 + 8 files changed, 175 insertions(+), 144 deletions(-) create mode 100644 imuboard/gps.c create mode 100644 imuboard/gps.h diff --git a/imuboard/Makefile b/imuboard/Makefile index 4ab55ad..cdc4913 100644 --- a/imuboard/Makefile +++ b/imuboard/Makefile @@ -17,6 +17,7 @@ SRC += fat.c SRC += sd_main.c SRC += partition.c SRC += sd_raw.c +SRC += gps.c SRC += gps_venus.c SRC += sd_log.c SRC += i2c_protocol.c diff --git a/imuboard/commands.c b/imuboard/commands.c index ab4132e..447e399 100644 --- a/imuboard/commands.c +++ b/imuboard/commands.c @@ -41,7 +41,7 @@ #include "main.h" #include "cmdline.h" #include "eeprom_config.h" -#include "gps_venus.h" +#include "gps.h" #include "imu.h" /* commands_gen.c */ diff --git a/imuboard/gps.c b/imuboard/gps.c new file mode 100644 index 0000000..65b4c28 --- /dev/null +++ b/imuboard/gps.c @@ -0,0 +1,110 @@ +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "gps.h" +#include "fat.h" +#include "fat_config.h" +#include "partition.h" +#include "sd_raw.h" +#include "sd_raw_config.h" +#include "sd_log.h" + +/* global gps position */ +struct gps_pos gps_pos; + +/* note: enabling debug will make the code fail due to rx queue full */ +#define debug_printf(fmt, a...) do { } while (0) +/* #define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__) */ + +/* display current GPS position stored in the global variable */ +void display_gps_pos(void) +{ + /* no need to call ntohs/ntohl because the frame is already fixed by + * decode_gps_pos() */ + printf_P(PSTR("id %.2X mode %.2X svnum %.2X gpsw %.4X " + "tow %.10"PRIu32"\t"), + gps_pos.msg_id, + gps_pos.mode, + gps_pos.sv_num, + gps_pos.gps_week, + gps_pos.tow); + + printf_P(PSTR("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n"), + gps_pos.latitude, + gps_pos.longitude, + gps_pos.altitude); + + printf_P(PSTR("gdop %3.3f pdop %3.3f hdop %3.3f vdop %3.3f " + "tdop %3.3f\n"), + (double)gps_pos.gdop/100., + (double)gps_pos.pdop/100., + (double)gps_pos.hdop/100., + (double)gps_pos.vdop/100., + (double)gps_pos.tdop/100.); + + printf_P(PSTR("lat %3.5f long %3.5f alt %3.5f sea_alt %3.5f\n"), + (double)gps_pos.latitude/10000000., + (double)gps_pos.longitude/10000000., + (double)gps_pos.altitude/100., + (double)gps_pos.sea_altitude/100.); + + printf_P(PSTR("vx %3.3f vy %3.3f vz %3.3f\n"), + (double)gps_pos.ecef_vx/100., + (double)gps_pos.ecef_vy/100., + (double)gps_pos.ecef_vz/100.); +} + +void gps_get_pos(struct gps_pos *pos) +{ + memcpy(pos, &gps_pos, sizeof(*pos)); +} + +int gps_log(uint8_t to_stdout) +{ + uint32_t ms; + uint8_t flags, prio; + int16_t len; + char buf[128]; + struct gps_pos pos; + + IRQ_LOCK(flags); + ms = global_ms; + IRQ_UNLOCK(flags); + + /* get position (prevent modification of gps pos during copy) */ + prio = callout_mgr_set_prio(&imuboard.intr_cm, GPS_PRIO); + gps_get_pos(&pos); + callout_mgr_restore_prio(&imuboard.intr_cm, prio); + + /* XXX copy */ + len = snprintf(buf, sizeof(buf), + "%"PRIu32" " + "svnum %.2X lat %3.5f long %3.5f " + "alt %3.5f sea_alt %3.5f\n", + ms, gps_pos.sv_num, + (double)gps_pos.latitude / 10000000., + (double)gps_pos.longitude / 10000000., + (double)gps_pos.altitude / 100., + (double)gps_pos.sea_altitude / 100.); + + + if (!to_stdout && sd_log_enabled()) { + if (sd_log_write(buf, len) != len) { + printf_P(PSTR("error writing to file\n")); + return -1; + } + } + else if (to_stdout) { + printf_P(PSTR("%s"), buf); + } + + return 0; +} diff --git a/imuboard/gps.h b/imuboard/gps.h new file mode 100644 index 0000000..539d8de --- /dev/null +++ b/imuboard/gps.h @@ -0,0 +1,59 @@ +#ifndef _GPS_H +#define _GPS_H + +#include + +/* A GPS position structure. It also contains some information about the number + * of seen satellites, the message ID, the date, ... + * inspired by venus gps, but tries to be quite generic */ +struct gps_pos { + uint8_t msg_id; /* should be A8 */ + uint8_t mode; /* Quality of fix 0: none, 1: 2D, 2: 3D, 3: 3D+DGNSS */ + uint8_t sv_num; /* number of SV in fix (0-12) */ + + uint16_t gps_week; /* GNSS week number */ + uint32_t tow; /* GNSS time of week */ + + int32_t latitude; /* between -90e7 and 90e7, in 1/1e-7 degrees, + * positive means north hemisphere */ + int32_t longitude; /* between -180e7 and 180e7, in 1/1e-7 degrees, + * positive means east */ + uint32_t altitude; /* altitude from elipsoid, in 1/100 meters */ + uint32_t sea_altitude; /* altitude from sea level, in 1/100 meters */ + + uint16_t gdop; /* Geometric dilution of precision */ + uint16_t pdop; /* Position dilution of precision */ + uint16_t hdop; /* Horizontal dilution of precision */ + uint16_t vdop; /* Vertical dilution of precision */ + uint16_t tdop; /* Timec dilution of precision */ + + int32_t ecef_x; /* Earth-Centered, Earth-Fixed X pos, 1/100 meters */ + int32_t ecef_y; /* Earth-Centered, Earth-Fixed Y pos, 1/100 meters */ + int32_t ecef_z; /* Earth-Centered, Earth-Fixed Z pos, 1/100 meters */ + + int32_t ecef_vx; /* Earth-Centered, Earth-Fixed X speed, 1/100 m/s */ + int32_t ecef_vy; /* Earth-Centered, Earth-Fixed Y speed, 1/100 m/s */ + int32_t ecef_vz; /* Earth-Centered, Earth-Fixed Z speed, 1/100 m/s */ + +} __attribute__ ((packed)); + +/* display global gps position */ +void display_gps_pos(void); + +/* log gps position stdout (if stdout is 1) or on sdcard (if 0) */ +int gps_log(uint8_t to_stdout); + +/* does not lock intr, must be done by user */ +void gps_get_pos(struct gps_pos *pos); + +static inline int8_t gps_pos_valid(struct gps_pos *pos) +{ + /* XXX when a GPS position is valid ? */ + return (pos->mode >= 2 && pos->sv_num >= 5); +} + +/* global gps position, use gps_get_pos() to copy it */ +extern struct gps_pos gps_pos; + + +#endif diff --git a/imuboard/gps_venus.c b/imuboard/gps_venus.c index 0580fa7..5d8f62b 100644 --- a/imuboard/gps_venus.c +++ b/imuboard/gps_venus.c @@ -10,13 +10,8 @@ #include #include "main.h" +#include "gps.h" #include "gps_venus.h" -#include "fat.h" -#include "fat_config.h" -#include "partition.h" -#include "sd_raw.h" -#include "sd_raw_config.h" -#include "sd_log.h" /* note: enabling debug will make the code fail due to rx queue full */ #define debug_printf(fmt, a...) do { } while (0) @@ -59,8 +54,6 @@ struct gps_frame { static struct gps_frame rxframe; -static struct gps_pos gps_pos; - static struct callout gps_timer; /* INPUT FORMATS */ @@ -535,44 +528,6 @@ static int decode_gps_pos(uint8_t *buf, uint16_t len) return 0; } -/* display current GPS position stored in the global variable */ -static void display_gps(void) -{ - /* no need to call ntohs/ntohl because the frame is already fixed by - * decode_gps_pos() */ - printf_P(PSTR("id %.2X mode %.2X svnum %.2X gpsw %.4X " - "tow %.10"PRIu32"\t"), - gps_pos.msg_id, - gps_pos.mode, - gps_pos.sv_num, - gps_pos.gps_week, - gps_pos.tow); - - printf_P(PSTR("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n"), - gps_pos.latitude, - gps_pos.longitude, - gps_pos.altitude); - - printf_P(PSTR("gdop %3.3f pdop %3.3f hdop %3.3f vdop %3.3f " - "tdop %3.3f\n"), - (double)gps_pos.gdop/100., - (double)gps_pos.pdop/100., - (double)gps_pos.hdop/100., - (double)gps_pos.vdop/100., - (double)gps_pos.tdop/100.); - - printf_P(PSTR("lat %3.5f long %3.5f alt %3.5f sea_alt %3.5f\n"), - (double)gps_pos.latitude/10000000., - (double)gps_pos.longitude/10000000., - (double)gps_pos.altitude/100., - (double)gps_pos.sea_altitude/100.); - - printf_P(PSTR("vx %3.3f vy %3.3f vz %3.3f\n"), - (double)gps_pos.ecef_vx/100., - (double)gps_pos.ecef_vy/100., - (double)gps_pos.ecef_vz/100.); -} - /* called on timer event */ static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg) { @@ -600,7 +555,7 @@ static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg) if (ret == 0) { decode_gps_pos(rxframe.data, rxframe.len); if (0) - display_gps(); + display_gps_pos(); last_valid = ms; is_valid = 1; @@ -664,50 +619,3 @@ int gps_venus_init(void) return 0; } - -void gps_get_pos(struct gps_pos *pos) -{ - memcpy(pos, &gps_pos, sizeof(*pos)); -} - -int gps_log(uint8_t to_stdout) -{ - uint32_t ms; - uint8_t flags, prio; - int16_t len; - char buf[128]; - struct gps_pos pos; - - IRQ_LOCK(flags); - ms = global_ms; - IRQ_UNLOCK(flags); - - /* get position (prevent modification of gps pos during copy) */ - prio = callout_mgr_set_prio(&imuboard.intr_cm, GPS_PRIO); - gps_get_pos(&pos); - callout_mgr_restore_prio(&imuboard.intr_cm, prio); - - /* XXX copy */ - len = snprintf(buf, sizeof(buf), - "%"PRIu32" " - "svnum %.2X lat %3.5f long %3.5f " - "alt %3.5f sea_alt %3.5f\n", - ms, gps_pos.sv_num, - (double)gps_pos.latitude / 10000000., - (double)gps_pos.longitude / 10000000., - (double)gps_pos.altitude / 100., - (double)gps_pos.sea_altitude / 100.); - - - if (!to_stdout && sd_log_enabled()) { - if (sd_log_write(buf, len) != len) { - printf_P(PSTR("error writing to file\n")); - return -1; - } - } - else if (to_stdout) { - printf_P(PSTR("%s"), buf); - } - - return 0; -} diff --git a/imuboard/gps_venus.h b/imuboard/gps_venus.h index 7397cbc..26b4e0c 100644 --- a/imuboard/gps_venus.h +++ b/imuboard/gps_venus.h @@ -1,54 +1,6 @@ #ifndef _GPS_VENUS_H #define _GPS_VENUS_H -#include - -/* A GPS position structure. It also contains some information about the number - * of seen satellites, the message ID, the date, ... - * See App Notes AN0028 p68 */ -struct gps_pos { - uint8_t msg_id; /* should be A8 */ - uint8_t mode; /* Quality of fix 0: none, 1: 2D, 2: 3D, 3: 3D+DGNSS */ - uint8_t sv_num; /* number of SV in fix (0-12) */ - - uint16_t gps_week; /* GNSS week number */ - uint32_t tow; /* GNSS time of week */ - - int32_t latitude; /* between -90e7 and 90e7, in 1/1e-7 degrees, - * positive means north hemisphere */ - int32_t longitude; /* between -180e7 and 180e7, in 1/1e-7 degrees, - * positive means east */ - uint32_t altitude; /* altitude from elipsoid, in 1/100 meters */ - uint32_t sea_altitude; /* altitude from sea level, in 1/100 meters */ - - uint16_t gdop; /* Geometric dilution of precision */ - uint16_t pdop; /* Position dilution of precision */ - uint16_t hdop; /* Horizontal dilution of precision */ - uint16_t vdop; /* Vertical dilution of precision */ - uint16_t tdop; /* Timec dilution of precision */ - - int32_t ecef_x; /* Earth-Centered, Earth-Fixed X pos, 1/100 meters */ - int32_t ecef_y; /* Earth-Centered, Earth-Fixed Y pos, 1/100 meters */ - int32_t ecef_z; /* Earth-Centered, Earth-Fixed Z pos, 1/100 meters */ - - int32_t ecef_vx; /* Earth-Centered, Earth-Fixed X speed, 1/100 m/s */ - int32_t ecef_vy; /* Earth-Centered, Earth-Fixed Y speed, 1/100 m/s */ - int32_t ecef_vz; /* Earth-Centered, Earth-Fixed Z speed, 1/100 m/s */ - -} __attribute__ ((packed)); - int gps_venus_init(void); -/* log gps position stdout (if stdout is 1) or on sdcard (if 0) */ -int gps_log(uint8_t to_stdout); - -/* does not lock intr, must be done by user */ -void gps_get_pos(struct gps_pos *pos); - -static inline int8_t gps_pos_valid(struct gps_pos *pos) -{ - /* XXX when a GPS position is valid ? */ - return (pos->mode >= 2 && pos->sv_num >= 5); -} - #endif diff --git a/imuboard/i2c_protocol.c b/imuboard/i2c_protocol.c index 3edb9b4..7d5a459 100644 --- a/imuboard/i2c_protocol.c +++ b/imuboard/i2c_protocol.c @@ -33,7 +33,7 @@ #include #include "../common/i2c_commands.h" -#include "gps_venus.h" +#include "gps.h" #include "imu.h" #include "main.h" diff --git a/imuboard/main.c b/imuboard/main.c index 37be46c..b257c65 100644 --- a/imuboard/main.c +++ b/imuboard/main.c @@ -54,6 +54,7 @@ #include #include "eeprom_config.h" +#include "gps.h" #include "gps_venus.h" #include "sd_log.h" #include "../common/i2c_commands.h" -- 2.39.5