Closes #374: Implement range module that centralizes ranging measurement

Makefile

@@ -152,6 +152,7 @@ PROJ_OBJ_CF2 += vl53l1_register_funcs.o vl53l1_wait.o vl53l1_core_support.o
 PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param.o
 PROJ_OBJ += log.o worker.o trigger.o sitaw.o queuemonitor.o msp.o
 PROJ_OBJ_CF2 += platformservice.o sound_cf2.o extrx.o sysload.o mem_cf2.o
+PROJ_OBJ += range.o
 
 # Stabilizer modules
 PROJ_OBJ += commander.o crtp_commander.o crtp_commander_rpyt.o

src/deck/drivers/src/multiranger.c

@@ -33,6 +33,7 @@
 #include "log.h"
 #include "pca95x4.h"
 #include "vl53l1x.h"
+#include "range.h"
 
 #include "i2cdev.h"
 
@@ -56,12 +57,6 @@ static VL53L1_Dev_t devUp;
 static VL53L1_Dev_t devLeft;
 static VL53L1_Dev_t devRight;
 
-static uint16_t rangeFront;
-static uint16_t rangeBack;
-static uint16_t rangeUp;
-static uint16_t rangeLeft;
-static uint16_t rangeRight;
-
 static uint16_t oaGetMeasurementAndRestart(VL53L1_Dev_t *dev)
 {
     VL53L1_Error status = VL53L1_ERROR_NONE;
@@ -110,11 +105,11 @@ static void oaTask(void *param)
     {
         vTaskDelayUntil(&lastWakeTime, M2T(100));
 
-        rangeFront = oaGetMeasurementAndRestart(&devFront);
-        rangeBack = oaGetMeasurementAndRestart(&devBack);
-        rangeUp = oaGetMeasurementAndRestart(&devUp);
-        rangeLeft = oaGetMeasurementAndRestart(&devLeft);
-        rangeRight = oaGetMeasurementAndRestart(&devRight);
+        rangeSet(rangeFront, oaGetMeasurementAndRestart(&devFront)/1000.0f);
+        rangeSet(rangeBack, oaGetMeasurementAndRestart(&devBack)/1000.0f);
+        rangeSet(rangeUp, oaGetMeasurementAndRestart(&devUp)/1000.0f);
+        rangeSet(rangeLeft, oaGetMeasurementAndRestart(&devLeft)/1000.0f);
+        rangeSet(rangeRight, oaGetMeasurementAndRestart(&devRight)/1000.0f);
     }
 }
 
@@ -228,14 +223,6 @@ static const DeckDriver multiranger_deck = {
 
 DECK_DRIVER(multiranger_deck);
 
-LOG_GROUP_START(range)
-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(range)
-
 PARAM_GROUP_START(deck)
 PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcMultiranger, &isInit)
 PARAM_GROUP_STOP(deck)

src/deck/drivers/src/zranger.c

@@ -34,6 +34,7 @@
 #include "debug.h"
 #include "log.h"
 #include "param.h"
+#include "range.h"
 
 #include "i2cdev.h"
 #include "zranger.h"
@@ -102,6 +103,7 @@ void zRangerTask(void* arg)
     vTaskDelayUntil(&xLastWakeTime, M2T(dev.measurement_timing_budget_ms));
 
     range_last = vl53l0xReadRangeContinuousMillimeters(&dev);
+    rangeSet(rangeDown, range_last / 1000.0f);
 
     // check if range is feasible and push into the kalman filter
     // the sensor should not be able to measure >3 [m], and outliers typically
@@ -147,7 +149,3 @@ DECK_DRIVER(zranger_deck);
 PARAM_GROUP_START(deck)
 PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcZRanger, &isInit)
 PARAM_GROUP_STOP(deck)
-
-LOG_GROUP_START(range)
-LOG_ADD(LOG_UINT16, zrange, &range_last)
-LOG_GROUP_STOP(range)

src/deck/drivers/src/zranger2.c

@@ -35,6 +35,7 @@
 #include "debug.h"
 #include "log.h"
 #include "param.h"
+#include "range.h"
 
 #include "i2cdev.h"
 #include "zranger2.h"
@@ -131,6 +132,7 @@ void zRanger2Task(void* arg)
     vTaskDelayUntil(&lastWakeTime, M2T(100));
 
     range_last = zRanger2GetMeasurementAndRestart(&dev);
+    rangeSet(rangeDown, range_last / 1000.0f);
 
     // check if range is feasible and push into the kalman filter
     // the sensor should not be able to measure >5 [m], and outliers typically
@@ -176,7 +178,3 @@ DECK_DRIVER(zranger2_deck);
 PARAM_GROUP_START(deck)
 PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcZRanger2, &isInit)
 PARAM_GROUP_STOP(deck)
-
-LOG_GROUP_START(range)
-LOG_ADD(LOG_UINT16, zrange, &range_last)
-LOG_GROUP_STOP(range)

src/modules/interface/range.h

@@ -0,0 +1,54 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie Firmware
+ *
+ * Copyright (C) 2018 Bitcraze AB
+ *
+ * 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, in version 3.
+ *
+ * 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/>.
+ *
+ * ranges.h: Centralize range measurements for different directions
+ *           and make them available as log
+ */
+
+#pragma once
+
+typedef enum {
+    rangeFront=0,
+    rangeBack,
+    rangeLeft,
+    rangeRight,
+    rangeUp,
+    rangeDown,
+    RANGE_T_END,
+} rangeDirection_t;
+
+/**
+ * Set the range for a certain direction
+ * 
+ * @param direction Direction of the range
+ * @param range_m Distance to an object in meter
+ */
+void rangeSet(rangeDirection_t direction, float range_m);
+
+/**
+ * Get the range for a certain direction
+ * 
+ * @param direction Direction of the range
+ * @return Distance to an object in meter
+ */
+float rangeGet(rangeDirection_t direction);

src/modules/src/range.c

@@ -0,0 +1,56 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie Firmware
+ *
+ * Copyright (C) 2018 Bitcraze AB
+ *
+ * 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, in version 3.
+ *
+ * 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/>.
+ *
+ * ranges.c: Centralize range measurements for different directions
+ *           and make them available as log
+ */
+#include <stdint.h>
+
+#include "log.h"
+
+#include "range.h"
+
+static uint16_t ranges[RANGE_T_END] = {0,};
+
+void rangeSet(rangeDirection_t direction, float range_m)
+{
+  if (direction > (RANGE_T_END-1)) return;
+
+  ranges[direction] = range_m * 1000;
+}
+
+float rangeGet(rangeDirection_t direction)
+{
+    if (direction > (RANGE_T_END-1)) return 0;
+
+  return ranges[direction];
+}
+
+LOG_GROUP_START(range)
+LOG_ADD(LOG_UINT16, front, &ranges[rangeFront])
+LOG_ADD(LOG_UINT16, back, &ranges[rangeBack])
+LOG_ADD(LOG_UINT16, up, &ranges[rangeUp])
+LOG_ADD(LOG_UINT16, left, &ranges[rangeLeft])
+LOG_ADD(LOG_UINT16, right, &ranges[rangeRight])
+LOG_ADD(LOG_UINT16, zrange, &ranges[rangeDown])
+LOG_GROUP_STOP(range)