From: Olivier Matz <zer0@droids-corp.org>
Date: Tue, 9 Sep 2014 17:02:19 +0000 (+0200)
Subject: beep when GPS returns a bad position
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=3c3e4a0489699413c1890dac09ab94fb00eac242;p=fpv.git

beep when GPS returns a bad position
---

diff --git a/common/i2c_commands.h b/common/i2c_commands.h
index 92a1405..7a9b9c5 100644
--- a/common/i2c_commands.h
+++ b/common/i2c_commands.h
@@ -58,8 +58,10 @@ struct i2c_req_imuboard_status {
 struct i2c_ans_imuboard_status {
 	struct i2c_cmd_hdr hdr;
 
-#define I2C_IMUBOARD_STATUS_GPS_OK 0x01
-#define I2C_IMUBOARD_STATUS_IMU_OK 0x02
+#define I2C_IMUBOARD_STATUS_BOOT_OK     0x01
+#define I2C_IMUBOARD_STATUS_SDCARD_OK   0x02
+#define I2C_IMUBOARD_STATUS_GPS_OK      0x04
+#define I2C_IMUBOARD_STATUS_IMU_OK      0x08
 	uint8_t flags;
 
 	int32_t latitude;  /* between -90e7 and 90e7, in 1/1e-7 degrees,
diff --git a/imuboard/i2c_protocol.c b/imuboard/i2c_protocol.c
index 9d73252..015e49c 100644
--- a/imuboard/i2c_protocol.c
+++ b/imuboard/i2c_protocol.c
@@ -83,6 +83,11 @@ static void i2c_send_status(void)
 	IRQ_UNLOCK(irq_flags);
 
 	ans.flags = 0;
+	if (imuboard.flags & IMUBOARD_F_BOOT_OK)
+		ans.flags |= I2C_IMUBOARD_STATUS_BOOT_OK;
+	if (imuboard.flags & IMUBOARD_F_SDCARD_OK)
+		ans.flags |= I2C_IMUBOARD_STATUS_SDCARD_OK;
+
 	if (gps_pos_valid(&gps_pos)) {
 		ans.flags |= I2C_IMUBOARD_STATUS_GPS_OK;
 		ans.latitude = gps_pos.latitude;
diff --git a/imuboard/main.h b/imuboard/main.h
index ad65316..e172287 100644
--- a/imuboard/main.h
+++ b/imuboard/main.h
@@ -72,6 +72,10 @@ struct imuboard {
 
 	struct callout_mgr intr_cm;
 
+#define IMUBOARD_F_BOOT_OK   0x01
+#define IMUBOARD_F_SDCARD_OK 0x02
+	uint8_t flags;
+
 	/* log */
 	uint8_t logs[NB_LOGS+1];
 	uint8_t log_level;
diff --git a/imuboard/sd_log.c b/imuboard/sd_log.c
index edc83ea..e7ff850 100644
--- a/imuboard/sd_log.c
+++ b/imuboard/sd_log.c
@@ -9,6 +9,7 @@
 #include "sd_raw.h"
 #include "sd_raw_config.h"
 #include "sd_log.h"
+#include "main.h"
 
 static struct fat_file_struct *log_fd = NULL;
 
@@ -127,7 +128,9 @@ int8_t sd_log_open(void)
 		printf_P(PSTR("error opening "));
 		return -1;
 	}
+	log_fd = fd;
 
+	imuboard.flags |= IMUBOARD_F_SDCARD_OK;
 	return 0;
 }
 
diff --git a/mainboard/i2c_protocol.c b/mainboard/i2c_protocol.c
index 912f769..fe58b7c 100644
--- a/mainboard/i2c_protocol.c
+++ b/mainboard/i2c_protocol.c
@@ -40,14 +40,14 @@
 #define I2C_TIMEOUT 100 /* ms */
 #define I2C_MAX_ERRORS 40
 
+#define IMUBOARD_BEEP_PERIOD_MS 2000
+
 static volatile uint8_t i2c_poll_num = 0;
 static volatile uint8_t i2c_state = 0;
 static volatile uint8_t i2c_rx_count = 0;
 static volatile uint8_t i2c_tx_count = 0;
 static volatile uint16_t i2c_errors = 0;
 
-static uint8_t gps_ok = 0;
-
 #define OP_READY 0 /* no i2c op running */
 #define OP_POLL  1 /* a user command is running */
 #define OP_CMD   2 /* a polling (req / ans) is running */
@@ -58,6 +58,7 @@ static volatile uint8_t running_op = OP_READY;
 static uint8_t error_log = 0;
 
 static struct callout i2c_timer;
+static struct callout imuboard_beep_timer;
 
 static int8_t i2c_req_imuboard_status(void);
 
@@ -112,6 +113,35 @@ void i2cproto_wait_update(void)
 	//WAIT_COND_OR_TIMEOUT((i2c_poll_num-poll_num) > 1, 150); /* XXX todo */
 }
 
+static void imuboard_beep_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
+{
+	(void)cm;
+	(void)tim;
+	(void)arg;
+
+	if ((imuboard_status.flags & I2C_IMUBOARD_STATUS_SDCARD_OK) == 0) {
+		beep(0, 1, 1);
+		goto reschedule;
+	}
+
+	if ((imuboard_status.flags & I2C_IMUBOARD_STATUS_BOOT_OK) == 0) {
+		beep(0, 1, 1);
+		beep(0, 1, 1);
+		goto reschedule;
+	}
+
+	if ((imuboard_status.flags & I2C_IMUBOARD_STATUS_GPS_OK) == 0) {
+		beep(0, 1, 1);
+		beep(0, 1, 1);
+		beep(0, 1, 1);
+		goto reschedule;
+	}
+
+ reschedule:
+	callout_reschedule(cm, tim, IMUBOARD_BEEP_PERIOD_MS);
+}
+
+
 /* called periodically : the goal of this 'thread' is to send requests
  * and read answers on i2c slaves in the correct order. */
 static void i2c_poll_slaves(struct callout_mgr *cm, struct callout *tim, void *arg)
@@ -252,13 +282,6 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 		/* copy status in a global struct */
 		memcpy(&imuboard_status, ans, sizeof(imuboard_status));
 
-		if (gps_ok == 0 &&
-			(imuboard_status.flags & I2C_IMUBOARD_STATUS_GPS_OK)) {
-			gps_ok = 1;
-			beep(0, 1, 1);
-			beep(0, 1, 1);
-		}
-
 		break;
 	}
 
@@ -338,4 +361,7 @@ void i2c_protocol_init(void)
 {
 	callout_init(&i2c_timer, i2c_poll_slaves, NULL, I2C_PRIO);
 	callout_schedule(&xbeeboard.intr_cm, &i2c_timer, I2C_PERIOD_MS);
+	callout_init(&imuboard_beep_timer, imuboard_beep_cb, NULL, I2C_PRIO); // prio ok ? XXX
+	callout_schedule(&xbeeboard.intr_cm, &imuboard_beep_timer,
+		IMUBOARD_BEEP_PERIOD_MS);
 }