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
#include "main.h"
#include "cmdline.h"
#include "eeprom_config.h"
-#include "gps_venus.h"
+#include "gps.h"
#include "imu.h"
/* commands_gen.c */
--- /dev/null
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/endian.h>
+#include <aversive/wait.h>
+#include <callout.h>
+
+#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;
+}
--- /dev/null
+#ifndef _GPS_H
+#define _GPS_H
+
+#include <stdint.h>
+
+/* 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
#include <uart.h>
#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)
static struct gps_frame rxframe;
-static struct gps_pos gps_pos;
-
static struct callout gps_timer;
/* INPUT FORMATS */
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)
{
if (ret == 0) {
decode_gps_pos(rxframe.data, rxframe.len);
if (0)
- display_gps();
+ display_gps_pos();
last_valid = ms;
is_valid = 1;
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;
-}
#ifndef _GPS_VENUS_H
#define _GPS_VENUS_H
-#include <stdint.h>
-
-/* 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
#include <i2c.h>
#include "../common/i2c_commands.h"
-#include "gps_venus.h"
+#include "gps.h"
#include "imu.h"
#include "main.h"
#include <i2c.h>
#include "eeprom_config.h"
+#include "gps.h"
#include "gps_venus.h"
#include "sd_log.h"
#include "../common/i2c_commands.h"
git.droids-corp.org / fpv.git / commitdiff