From cbb06cf32846f2028d37430b96f00891d1dfc46a Mon Sep 17 00:00:00 2001
From: Arnaud Taffanel <arnaud@bitcraze.se>
Date: Thu, 5 Oct 2017 14:25:29 +0200
Subject: [PATCH] Add OA deck driver (#254)

---
 Makefile                  |   3 +-
 src/deck/drivers/src/oa.c | 148 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 150 insertions(+), 1 deletion(-)
 create mode 100644 src/deck/drivers/src/oa.c

diff --git a/Makefile b/Makefile
index 8325e767fe..faa9fe87a5 100644
--- a/Makefile
+++ b/Makefile
@@ -124,7 +124,7 @@ PROJ_OBJ_CF2 += ak8963.o eeprom.o maxsonar.o piezo.o
 PROJ_OBJ_CF2 += uart_syslink.o swd.o uart1.o uart2.o watchdog.o
 PROJ_OBJ_CF2 += cppm.o
 PROJ_OBJ_CF2 += bmi055_accel.o bmi055_gyro.o bmi160.o bmp280.o bstdr_comm_support.o bmm150.o
-PROJ_OBJ_CF2 += pca9685.o vl53l0x.o
+PROJ_OBJ_CF2 += pca9685.o vl53l0x.o pca95x4.o
 # USB Files
 PROJ_OBJ_CF2 += usb_bsp.o usblink.o usbd_desc.o usb.o
 
@@ -173,6 +173,7 @@ PROJ_OBJ_CF2 += zranger.o
 PROJ_OBJ_CF2 += locodeck.o
 PROJ_OBJ_CF2 += lpsTwrTag.o
 PROJ_OBJ_CF2 += flowdeck.o
+PROJ_OBJ_CF2 += oa.o
 
 ifeq ($(LPS_TDOA_ENABLE), 1)
 PROJ_OBJ_CF2 += lpsTdoaTag.o
diff --git a/src/deck/drivers/src/oa.c b/src/deck/drivers/src/oa.c
new file mode 100644
index 0000000000..2eb536ae08
--- /dev/null
+++ b/src/deck/drivers/src/oa.c
@@ -0,0 +1,148 @@
+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * LPS node firmware.
+ *
+ * Copyright 2017, Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Foobar 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 Foobar.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/* oa.c: Codename Obstacle Avoidance driver */
+#include "deck.h"
+
+#include "system.h"
+#include "log.h"
+#include "pca95x4.h"
+#include "vl53l0x.h"
+
+#include "i2cdev.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include <stdlib.h>
+
+static bool isInit = false;
+
+#define OA_PIN_UP     PCA95X4_P0
+#define OA_PIN_FRONT  PCA95X4_P4
+#define OA_PIN_BACK   PCA95X4_P1
+#define OA_PIN_LEFT   PCA95X4_P6
+#define OA_PIN_RIGHT  PCA95X4_P2
+
+static VL53L0xDev devFront;
+static VL53L0xDev devBack;
+static VL53L0xDev devUp;
+static VL53L0xDev devLeft;
+static VL53L0xDev devRight;
+
+static uint16_t rangeFront;
+static uint16_t rangeBack;
+static uint16_t rangeUp;
+static uint16_t rangeLeft;
+static uint16_t rangeRight;
+
+static void oaTask(void *param)
+{
+  systemWaitStart();
+
+  vl53l0xStartContinuous(&devFront, 0);
+  vl53l0xStartContinuous(&devBack, 0);
+  vl53l0xStartContinuous(&devUp, 0);
+  vl53l0xStartContinuous(&devLeft, 0);
+  vl53l0xStartContinuous(&devRight, 0);
+
+  TickType_t lastWakeTime = xTaskGetTickCount();
+
+  while(1) {
+    vTaskDelayUntil(&lastWakeTime, M2T(50));
+
+    rangeFront = vl53l0xReadRangeContinuousMillimeters(&devFront);
+    rangeBack = vl53l0xReadRangeContinuousMillimeters(&devBack);
+    rangeUp = vl53l0xReadRangeContinuousMillimeters(&devUp);
+    rangeLeft = vl53l0xReadRangeContinuousMillimeters(&devLeft);
+    rangeRight = vl53l0xReadRangeContinuousMillimeters(&devRight);
+  }
+}
+
+static void oaInit()
+{
+  if (isInit) return;
+
+  pca95x4Init();
+
+  pca95x4ConfigOutput(~(OA_PIN_UP |
+                        OA_PIN_RIGHT |
+                        OA_PIN_LEFT |
+                        OA_PIN_FRONT |
+                        OA_PIN_BACK));
+
+  pca95x4ClearOutput(OA_PIN_UP |
+                     OA_PIN_RIGHT |
+                     OA_PIN_LEFT |
+                     OA_PIN_FRONT |
+                     OA_PIN_BACK);
+
+  isInit = true;
+
+  xTaskCreate(oaTask, "oa", 2*configMINIMAL_STACK_SIZE, NULL,
+              /*priority*/3, NULL);
+}
+
+static bool oaTest()
+{
+  bool pass = isInit;
+
+  pca95x4SetOutput(OA_PIN_FRONT);
+  pass &= vl53l0xInit(&devFront, I2C1_DEV, true);
+
+  pca95x4SetOutput(OA_PIN_BACK);
+  pass &= vl53l0xInit(&devBack, I2C1_DEV, true);
+
+  pca95x4SetOutput(OA_PIN_UP);
+  pass &= vl53l0xInit(&devUp, I2C1_DEV, true);
+
+  pca95x4SetOutput(OA_PIN_LEFT);
+  pass &= vl53l0xInit(&devLeft, I2C1_DEV, true);
+
+  pca95x4SetOutput(OA_PIN_RIGHT);
+  pass &= vl53l0xInit(&devRight, I2C1_DEV, true);
+
+  return pass;
+}
+
+static const DeckDriver oa_deck = {
+  .vid = 0xBC,
+  .pid = 0x0B,
+  .name = "bcOA",
+
+  .usedGpio = 0,  // FIXME: set the used pins
+
+  .init = oaInit,
+  .test = oaTest,
+};
+
+DECK_DRIVER(oa_deck);
+
+LOG_GROUP_START(oa)
+LOG_ADD(LOG_UINT16, front, &rangeFront)
+LOG_ADD(LOG_UINT16, back, &rangeBack)
+LOG_ADD(LOG_UINT16, up, &rangeUp)
+LOG_ADD(LOG_UINT16, left, &rangeLeft)
+LOG_ADD(LOG_UINT16, right, &rangeRight)
+LOG_GROUP_STOP(oa)