From 99a00646c85d806f34d7a4aa9f52b318607f7530 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Tue, 22 Jul 2014 23:53:05 +0200 Subject: [PATCH] gps: dump position in i2c message --- gps_venus.c | 52 ++++++++++++++++++++++++++---------------- gps_venus.h | 61 +++++++++++++++++++++++++++++--------------------- i2c_protocol.c | 26 +++++++++++---------- imu.c | 21 +++++++++++++++++ 4 files changed, 103 insertions(+), 57 deletions(-) diff --git a/gps_venus.c b/gps_venus.c index 01551a6..b3bbae3 100644 --- a/gps_venus.c +++ b/gps_venus.c @@ -97,8 +97,8 @@ typedef struct { -/* 0x08 - nema interval */ -#define M_NEMAINTERVAL 0x08 +/* 0x08 - nmea interval */ +#define M_NMEAINTERVAL 0x08 typedef struct { uint8_t gga; uint8_t gsa; @@ -112,7 +112,7 @@ typedef struct { 1 - flash */ -} m_nemainterval_t; +} m_nmeainterval_t; /* 0x09 - Query Software Version */ @@ -120,7 +120,7 @@ typedef struct { typedef struct { uint8_t msg_type; /* 00 = no output - 01 = nema output + 01 = nmea output 02 = binary output */ } m_output_t; @@ -238,13 +238,15 @@ void venus634_rate(void) memset(&rate,0,sizeof(m_rate_t)); rate.rate = 20; - //rate.rate = 0; /* ram */ + rate.attributes = 0; /* ram */ venus634_send(M_RATE,&rate,sizeof(m_rate_t)); return; } +/* Wide Area Augmentation System: use ground stations to increase the precision + * of gps position */ void venus634_waas(void) { m_waas_t waas; @@ -258,7 +260,7 @@ void venus634_waas(void) return; } - +/* Tell we are a car instead of a pedestrian */ void venus634_navmode(void) { m_navmode_t navmode; @@ -272,23 +274,26 @@ void venus634_navmode(void) return; } - -void venus634_nema_interval(void) +/* frequency of NMEA messages */ +void venus634_nmea_interval(void) { - m_nemainterval_t nemainterval; + m_nmeainterval_t nmeainterval; + + memset(&nmeainterval,0,sizeof(m_nmeainterval_t)); - memset(&nemainterval,0,sizeof(m_nemainterval_t)); - nemainterval.gga = 1; - nemainterval.gsa = 1; - nemainterval.gsv = 1; - nemainterval.gll = 1; - nemainterval.rmc = 1; - nemainterval.vtg = 1; - nemainterval.zda = 1; + /* set frequency for nmea: 1 = once every one position fix, 2 = once + * every two position fix, ... */ + nmeainterval.gga = 1; /* GPGGA interval - GPS Fix Data*/ + nmeainterval.gsa = 1; /* GNSS DOPS and Active Satellites */ + nmeainterval.gsv = 1; /* GNSS Satellites in View */ + nmeainterval.gll = 1; /* Geographic Position - Latitude longitude */ + nmeainterval.rmc = 1; /* Recomended Minimum Specific GNSS Sentence */ + nmeainterval.vtg = 1; /* Course Over Ground and Ground Speed */ + nmeainterval.zda = 1; /* Time & Date*/ - nemainterval.attributes = 1; /* ram flash */ + nmeainterval.attributes = 1; /* ram flash */ - venus634_send(M_NEMAINTERVAL,&nemainterval,sizeof(m_nemainterval_t)); + venus634_send(M_NMEAINTERVAL,&nmeainterval,sizeof(m_nmeainterval_t)); return; } @@ -408,10 +413,14 @@ int wait_ack(int msg_type) static int decode_gps_pos(uint8_t *buf, uint16_t len) { struct gps_pos *pos = (struct gps_pos *)buf; + uint8_t irq_flags; if (len != sizeof(*pos)) return -1; + if (pos->msg_id != 0xA8) /* XXX not tested */ + return -1; + gps_pos.gps_week = ntohs(pos->gps_week); gps_pos.tow = ntohl(pos->tow); @@ -431,6 +440,11 @@ static int decode_gps_pos(uint8_t *buf, uint16_t len) gps_pos.ecef_vy = ntohl(pos->ecef_vy); gps_pos.ecef_vz = ntohl(pos->ecef_vz); + /* update global structure */ + IRQ_LOCK(irq_flags); + memcpy(&gps_pos, pos, sizeof(gps_pos)); + IRQ_UNLOCK(irq_flags); + return 0; } diff --git a/gps_venus.h b/gps_venus.h index 641bcbb..ca2bd8f 100644 --- a/gps_venus.h +++ b/gps_venus.h @@ -4,33 +4,36 @@ #include /* A GPS position structure. It also contains some information about the number - * of seen satellites, the message ID, the date, .. */ + * of seen satellites, the message ID, the date, ... + * See App Notes AN0028 p68 */ struct gps_pos { - uint8_t msg_id; /* */ - uint8_t mode; - uint8_t sv_num; - uint16_t gps_week; - uint32_t tow; - - int32_t latitude; - int32_t longitude; - uint32_t altitude; - - uint32_t sea_altitude; - - uint16_t gdop; - uint16_t pdop; - uint16_t hdop; - uint16_t vdop; - uint16_t tdop; - - int32_t ecef_x; - int32_t ecef_y; - int32_t ecef_z; - - int32_t ecef_vx; - int32_t ecef_vy; - int32_t ecef_vz; + 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)); @@ -40,4 +43,10 @@ int gps_loop(void); /* 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/i2c_protocol.c b/i2c_protocol.c index f62bfb2..274e9f6 100644 --- a/i2c_protocol.c +++ b/i2c_protocol.c @@ -33,6 +33,7 @@ #include #include "../fpv-common/i2c_commands.h" +#include "gps_venus.h" #include "main.h" void i2c_protocol_init(void) @@ -70,25 +71,26 @@ static void i2c_test(uint16_t val) static void i2c_send_status(void) { struct i2c_ans_imuboard_status ans; - static uint8_t x; + struct gps_pos gps_pos; + uint8_t irq_flags; i2c_flush(); ans.hdr.cmd = I2C_ANS_IMUBOARD_STATUS; - /* status */ - ans.test = x++; /* XXX */ -#if 0 - ans.mode = state_get_mode(); - ans.status = state_get_status(); + /* gps */ + IRQ_LOCK(irq_flags); + gps_get_pos(&gps_pos); + IRQ_UNLOCK(irq_flags); - ans.left_cobroller_speed = imuboard.left_cobroller_speed; - ans.right_cobroller_speed = imuboard.right_cobroller_speed; - - ans.cob_count = state_get_cob_count(); -#endif + if (gps_pos_valid(&gps_pos)) { + ans.flags |= IMUBOARD_STATUS_GPS_OK; + ans.latitude = gps_pos.latitude; + ans.longitude = gps_pos.longitude; + ans.altitude = gps_pos.altitude; + } i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans, - sizeof(ans), I2C_CTRL_GENERIC); + sizeof(ans), I2C_CTRL_GENERIC); } void i2c_recvevent(uint8_t * buf, int8_t size) diff --git a/imu.c b/imu.c index 86d36f2..7e9a17f 100644 --- a/imu.c +++ b/imu.c @@ -44,12 +44,33 @@ static void quaternion2euler(const struct quaternion *quat, struct euler *euler) /* timer callback that polls IMU and does the calculation */ static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg) { + struct imu_info imu; + struct quaternion quat; + struct euler euler; + uint8_t irq_flags; + (void)arg; + /* copy previous quaternion locally */ + IRQ_LOCK(irq_flags); + memcpy(&quat, &g_quat, sizeof(quat)); + IRQ_UNLOCK(irq_flags); + mpu6050_read_all_axes(&g_imu); MadgwickAHRSupdate(&g_imu, &g_quat); quaternion2euler(&g_quat, &g_euler); + /* update global variables */ + IRQ_LOCK(irq_flags); + memcpy(&g_imu, &imu, sizeof(g_imu)); + IRQ_UNLOCK(irq_flags); + IRQ_LOCK(irq_flags); + memcpy(&g_quat, &quat, sizeof(g_quat)); + IRQ_UNLOCK(irq_flags); + IRQ_LOCK(irq_flags); + memcpy(&g_euler, &euler, sizeof(g_euler)); + IRQ_UNLOCK(irq_flags); + /* reschedule event */ callout_schedule(cm, tim, 2); } -- 2.20.1