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)