Synchronous gyro interrupts on flyingpi

flight/PiOS/Common/pios_bmx055.c

@@ -264,25 +264,38 @@ int32_t PIOS_BMX055_SPI_Init(pios_bmx055_dev_t *dev, pios_spi_t spi_id,
 
 	if (ret) return ret;
 
-#if 0
-	/* Set up EXTI line */
-	PIOS_EXTI_Init(bmx_dev->cfg->exti_cfg);
-
-	/* Wait 20 ms for data ready interrupt and make sure it happens twice */
-	if (!bmx_dev->cfg->skip_startup_irq_check) {
-		for (int i=0; i<2; i++) {
-			uint32_t ref_val = bmx_dev->interrupt_count;
-			uint32_t raw_start = PIOS_DELAY_GetRaw();
-
-			while (bmx_dev->interrupt_count == ref_val) {
-				if (PIOS_DELAY_DiffuS(raw_start) > 20000) {
-					PIOS_EXTI_DeInit(bmx_dev->cfg->exti_cfg);
-					return -PIOS_BMX_ERROR_NOIRQ;
+	if (bmx_dev->cfg->int_pin) {
+		dio_set_input(bmx_dev->cfg->int_pin, DIO_PULL_NONE);
+
+		ret = PIOS_BMX_WriteReg(bmx_dev->spi_slave_gyro,
+				BMX055_REG_GYRO_INT_EN_0,
+				128);
+
+		if (ret) return ret;
+
+		ret = PIOS_BMX_WriteReg(bmx_dev->spi_slave_gyro,
+				BMX055_REG_GYRO_INT_EN_1,
+				12);
+
+		if (ret) return ret;
+
+		ret = PIOS_BMX_WriteReg(bmx_dev->spi_slave_gyro,
+				BMX055_REG_GYRO_INT_MAP_1,
+				1);
+
+		if (ret) return ret;
+
+		/* Wait 20 ms for data ready interrupt and make sure it happens
+		 * twice */
+		if (!bmx_dev->cfg->skip_startup_irq_check) {
+			for (int i=0; i<2; i++) {
+				if (!dio_wait(bmx_dev->cfg->int_pin, true, 20000)) {
+					DEBUG_PRINTF(2, "Missing interrupt!");
+					return -100;
 				}
 			}
 		}
 	}
-#endif
 
 	bmx_dev->task_handle = PIOS_Thread_Create(
 			PIOS_BMX_Task, "pios_bmx", PIOS_BMX_TASK_STACK,
@@ -293,8 +306,13 @@ int32_t PIOS_BMX055_SPI_Init(pios_bmx055_dev_t *dev, pios_spi_t spi_id,
 
 	PIOS_SENSORS_SetMaxGyro(2000);
 
-	PIOS_SENSORS_SetSampleRate(PIOS_SENSOR_GYRO, 800);
-	PIOS_SENSORS_SetSampleRate(PIOS_SENSOR_ACCEL, 800);
+	if (bmx_dev->cfg->int_pin) {
+		PIOS_SENSORS_SetSampleRate(PIOS_SENSOR_GYRO, 1000);
+		PIOS_SENSORS_SetSampleRate(PIOS_SENSOR_ACCEL, 1000);
+	} else {
+		PIOS_SENSORS_SetSampleRate(PIOS_SENSOR_GYRO, 800);
+		PIOS_SENSORS_SetSampleRate(PIOS_SENSOR_ACCEL, 800);
+	}
 
 	PIOS_SENSORS_Register(PIOS_SENSOR_ACCEL, bmx_dev->accel_queue);
 	PIOS_SENSORS_Register(PIOS_SENSOR_GYRO, bmx_dev->gyro_queue);
@@ -353,33 +371,16 @@ static int32_t PIOS_BMX_WriteReg(int slave, uint8_t address, uint8_t buffer)
 	return 0;
 }
 
-#if 0
-bool PIOS_BMX_IRQHandler(void)
-{
-	if (PIOS_BMX_Validate(bmx_dev) != 0)
-		return false;
-
-	bool woken = false;
-
-	bmx_dev->interrupt_count++;
-
-	PIOS_Semaphore_Give_FromISR(bmx_dev->data_ready_sema, &woken);
-
-	return woken;
-}
-#endif
-
 static void PIOS_BMX_Task(void *parameters)
 {
 	(void)parameters;
 
 	while (true) {
-#if 0
-		//Wait for data ready interrupt
-		if (PIOS_Semaphore_Take(bmx_dev->data_ready_sema, PIOS_SEMAPHORE_TIMEOUT_MAX) != true)
-			continue;
-#endif
-		PIOS_Thread_Sleep(1);		/* XXX */
+		if (!bmx_dev->cfg->int_pin) {
+			PIOS_Thread_Sleep(1);
+		} else {
+			dio_wait(bmx_dev->cfg->int_pin, true, 1000000);
+		}
 
 		// claim bus in high speed mode
 		if (PIOS_SPI_ClaimBus(bmx_dev->spi_id) != 0)

flight/PiOS/inc/pios_bmx055.h

@@ -29,6 +29,7 @@
 #define PIOS_BMX055_H
 
 #include "pios.h"
+#include <pios_dio.h>
 
 enum pios_bmx055_orientation { // clockwise rotation from board forward
 	PIOS_BMX_TOP_0DEG    = 0x00,
@@ -42,10 +43,10 @@ enum pios_bmx055_orientation { // clockwise rotation from board forward
 };
 
 struct pios_bmx055_cfg {
-//	const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */
-
 	enum pios_bmx055_orientation orientation;
 	bool skip_startup_irq_check;
+
+	dio_tag_t int_pin;
 };
 
 typedef struct pios_bmx055_dev * pios_bmx055_dev_t;
@@ -58,14 +59,6 @@ int32_t PIOS_BMX055_SPI_Init(pios_bmx055_dev_t *dev, pios_spi_t spi_id,
 		uint32_t slave_gyro, uint32_t slave_accel,
 		const struct pios_bmx055_cfg *cfg);
 
-#if 0
-/**
- * @brief The IMU interrupt handler.
- * Fetches new data from the IMU.
- */
-bool PIOS_BMX055_IRQHandler(void);
-#endif
-
 #endif /* PIOS_BMX055_H */
 
 /** 

flight/PiOS/posix/inc/pios_dio.h

@@ -0,0 +1,217 @@
+/**
+ ******************************************************************************
+ * @file       pios_dio.h
+ * @author     dRonin, http://dRonin.org/, Copyright (C) 2017
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_DIO Digital IO subsystem
+ * @{
+ *
+ * @brief Provides an interface to configure and efficiently use GPIOs.
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http://www.gnu.org/licenses/>
+ *
+ * Additional note on redistribution: The copyright and license notices above
+ * must be maintained in each individual source file that is a derivative work
+ * of this source file; otherwise redistribution is prohibited.
+ */
+
+#ifndef _PIOS_DIO_H
+#define _PIOS_DIO_H
+
+enum dio_drive_strength {
+	DIO_DRIVE_WEAK = 0,
+	DIO_DRIVE_LIGHT,
+	DIO_DRIVE_MEDIUM,
+	DIO_DRIVE_STRONG
+};
+
+enum dio_pin_function {
+	DIO_PIN_INPUT = 0,
+	DIO_PIN_OUTPUT = 1,
+};
+
+enum dio_pull {
+	DIO_PULL_NONE = 0,
+	DIO_PULL_UP,
+	DIO_PULL_DOWN
+};
+
+struct dio_info_s {
+	bool inited;
+
+	int num;
+
+	int val_fd;
+	int dir_fd;
+	int edge_fd;
+
+	bool out_val;
+	bool open_collector;
+	bool is_output;
+
+	char val_fname[256];
+};
+
+typedef struct dio_info_s *dio_tag_t;
+
+#ifdef PIOS_INCLUDE_DIO
+
+#include <sys/types.h>
+#include <unistd.h>
+#include <poll.h>
+#include <signal.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+#define DIO_MAKE_TAG(pin) dio_init(pin)
+
+dio_tag_t dio_init(int pin);
+
+/*
+#define DIO_MAKE_TAG(port, pin) ((uintptr_t) ((DIO_PORT_OFFSET(port) << 16) | ((uint16_t) (pin))))
+*/
+
+static inline void _dio_fd_write(int fd, const char *str)
+{
+	lseek(fd, 0, SEEK_SET);
+
+	int expect = strlen(str);
+	int ret;
+
+	ret = write(fd, str, expect);
+
+	(void) ret;
+}
+
+static inline void dio_write(dio_tag_t t, bool high)
+{
+	if (!t->is_output) {
+		return;
+	}
+
+	/* low/high/in/out can be written to direction; low/high imply
+	 * "set to output and drive to this value"
+	 */
+	if (high) {
+		if (t->open_collector) {
+			_dio_fd_write(t->dir_fd, "in");
+		} else {
+			_dio_fd_write(t->dir_fd, "high");
+		}
+	} else {
+		_dio_fd_write(t->dir_fd, "low");
+	}
+
+	t->out_val = high;
+}
+
+static inline void dio_high(dio_tag_t t)
+{
+	dio_write(t, true);
+}
+
+static inline void dio_low(dio_tag_t t)
+{
+	dio_write(t, false);
+}
+
+static inline void dio_toggle(dio_tag_t t)
+{
+	dio_write(t, !t->out_val);
+}
+
+static inline void dio_set_output(dio_tag_t t, bool open_collector,
+		enum dio_drive_strength strength, bool first_value)
+{
+	(void) strength;
+
+	t->is_output = true;
+	t->open_collector = open_collector;
+
+	dio_write(t, first_value);
+}
+
+static inline void dio_set_input(dio_tag_t t, enum dio_pull pull)
+{
+	(void) pull;
+
+	t->is_output = false;
+
+	printf("setting input\n");
+
+	_dio_fd_write(t->dir_fd, "in");
+}
+
+static inline bool dio_read(dio_tag_t t)
+{
+	lseek(t->val_fd, 0, SEEK_SET);
+
+	char buf[16];
+
+	int num = read(t->val_fd, buf, sizeof(buf));
+
+	if (num < 1) {
+		return false;	/* !!! */
+	}
+
+	if (buf[0] == '1') {
+		return true;
+	}
+
+	return false;
+}
+
+static inline bool dio_wait(dio_tag_t t, bool rising, int usec)
+{
+	if (rising) {
+		_dio_fd_write(t->edge_fd, "rising");
+	} else {
+		_dio_fd_write(t->edge_fd, "falling");
+	}
+
+	struct pollfd ev = {
+		.fd = t->val_fd,
+		.events = POLLPRI,
+	};
+
+	/* Consider ppoll / fancier stuff */
+	int timeout = (usec + 999) / 1000;
+
+	if (usec < 0) {
+		timeout = -1;
+	}
+
+	int ret = poll(&ev, 1, timeout);
+
+	if (ret == 1) {
+		/* need to 'reset' this */
+		dio_read(t);
+
+		return true;
+	}
+
+	return false;
+}
+
+#endif /* PIOS_INCLUDE_DIO */
+
+#endif /* _PIOS_DIO_H */
+
+/**
+ * @}
+ * @}
+ */