-/* 0x08 - nema interval */
-#define M_NEMAINTERVAL 0x08
+/* 0x08 - nmea interval */
+#define M_NMEAINTERVAL 0x08
typedef struct {
uint8_t gga;
uint8_t gsa;
1 - flash
*/
-} m_nemainterval_t;
+} m_nmeainterval_t;
/* 0x09 - Query Software Version */
typedef struct {
uint8_t msg_type; /*
00 = no output
- 01 = nema output
+ 01 = nmea output
02 = binary output
*/
} m_output_t;
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;
return;
}
-
+/* Tell we are a car instead of a pedestrian */
void venus634_navmode(void)
{
m_navmode_t navmode;
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;
}
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);
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;
}
#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 {
- 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));
/* 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 "../fpv-common/i2c_commands.h"
+#include "gps_venus.h"
#include "main.h"
void i2c_protocol_init(void)
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)
/* 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);
}
git.droids-corp.org / protos / imu.git / commitdiff