gps: dump position in i2c message

diff --git a/gps_venus.c b/gps_venus.c
index 01551a6..b3bbae3 100644 (file)
--- 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;
 typedef struct {
        uint8_t gga;
        uint8_t gsa;


@@ -112,7 +112,7 @@ typedef struct {
                              1 - flash
                             */
 
                              1 - flash
                             */
 

-} m_nemainterval_t;
+} m_nmeainterval_t;

 
 
 /* 0x09 - Query Software Version */
 
 
 /* 0x09 - Query Software Version */


@@ -120,7 +120,7 @@ typedef struct {
 typedef struct {
        uint8_t msg_type; /*
                            00 = no output
 typedef struct {
        uint8_t msg_type; /*
                            00 = no output

-                           01 = nema output
+                           01 = nmea output

                            02 = binary output
                           */
 } m_output_t;
                            02 = binary output
                           */
 } m_output_t;


@@ -238,13 +238,15 @@ void venus634_rate(void)
 
        memset(&rate,0,sizeof(m_rate_t));
        rate.rate = 20;
 
        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;
 }
 
 
        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;
 void venus634_waas(void)
 {
        m_waas_t waas;


@@ -258,7 +260,7 @@ void venus634_waas(void)
        return;
 }
 
        return;
 }
 

-
+/* Tell we are a car instead of a pedestrian */

 void venus634_navmode(void)
 {
        m_navmode_t navmode;
 void venus634_navmode(void)
 {
        m_navmode_t navmode;


@@ -272,23 +274,26 @@ void venus634_navmode(void)
        return;
 }
 
        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;
 }
 
        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;
 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 (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);
 
        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);
 
        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;
 }
 
        return 0;
 }
 

diff --git a/gps_venus.h b/gps_venus.h
index 641bcbb..ca2bd8f 100644 (file)
--- a/gps_venus.h
+++ b/gps_venus.h
@@ -4,33 +4,36 @@
 #include <stdint.h>
 
 /* A GPS position structure. It also contains some information about the number
 #include <stdint.h>
 
 /* 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 {
 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));
 
 
 } __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);
 
 /* 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
 #endif

diff --git a/i2c_protocol.c b/i2c_protocol.c
index f62bfb2..274e9f6 100644 (file)
--- a/i2c_protocol.c
+++ b/i2c_protocol.c
@@ -33,6 +33,7 @@
 #include <i2c.h>
 
 #include "../fpv-common/i2c_commands.h"
 #include <i2c.h>
 
 #include "../fpv-common/i2c_commands.h"

+#include "gps_venus.h"

 #include "main.h"
 
 void i2c_protocol_init(void)
 #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 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;
 
 
        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,
 
        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)
 }
 
 void i2c_recvevent(uint8_t * buf, int8_t size)

diff --git a/imu.c b/imu.c
index 86d36f2..7e9a17f 100644 (file)
--- 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)
 {
 /* 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;
 
        (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);
 
        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);
 }
        /* reschedule event */
        callout_schedule(cm, tim, 2);
 }