diff --git a/Makefile b/Makefile
index 190ad2e51c..e60e77cce5 100644
--- a/Makefile
+++ b/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
diff --git a/src/deck/drivers/src/multiranger.c b/src/deck/drivers/src/multiranger.c
index 411e70e764..38d4416c8d 100644
--- a/src/deck/drivers/src/multiranger.c
+++ b/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)
diff --git a/src/deck/drivers/src/zranger.c b/src/deck/drivers/src/zranger.c
index 5a889da10f..cd9c91de77 100644
--- a/src/deck/drivers/src/zranger.c
+++ b/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)
diff --git a/src/deck/drivers/src/zranger2.c b/src/deck/drivers/src/zranger2.c
index 13e5eae244..afa66d1ef5 100644
--- a/src/deck/drivers/src/zranger2.c
+++ b/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)
diff --git a/src/modules/interface/range.h b/src/modules/interface/range.h
new file mode 100644
index 0000000000..721c374c5b
--- /dev/null
+++ b/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 .
+ *
+ * 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);
\ No newline at end of file
diff --git a/src/modules/src/range.c b/src/modules/src/range.c
new file mode 100644
index 0000000000..66cf351fc8
--- /dev/null
+++ b/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 .
+ *
+ * ranges.c: Centralize range measurements for different directions
+ * and make them available as log
+ */
+#include
+
+#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)