#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)