Crazyflie 2.0 Custom Obstacle Avoidance Deck with 13 VL53L1X ToF Sensors
Crazyflie 2.0 is a lightweight micro aerial vehicle. This open source
platform comes with a variety of decks to support multiple
implementations. One of them is a Obstacle Avoidance Deck by the original manufacturer itself.
This
deck supports five Time of Flight (ToF) sensors mounted as shown in
figure below. Our project is requires sensor readings captured using
these ToF sensors. Effectiveness and accuracy of our algorithms require
more data points than points provided by just five sensors. There are no
other hardware platforms available in the Internet supporting more ToF
sensors and the alternative we had was to develop one on our own.
Researching
into existing hardware platform, we were able to extend the design to
build a custom deck that supports 13 ToF sensors mounted facing
different angles. The schematics and bill of materials can be found from
this repository hosted online: https://github.com/CloudyPadmal/CrazyFlieToFDeck
Designing
the hardware should be done in such a way that updating and testing the
firmware is possible and it will be functional. In the hardware design,
an LED was added to indicate test results status using a visual
approach. Due to difficulties in finding a one wire memory IC, we opt to force the deck instead.
Crazyflie-firmware supports this feature by adding a configuration setting in a make file. Create a new file in the following location and name it config.mk
crazyflie-firmware ->> tools -> make -> config.mk
There is a file with a name config.mk.example which contains all the possible commands we can add into the make file. In the newly created make file, add the following line and save it.
CFLAGS += -DDECK_FORCE=bcOA
Note that, bcOA is the name we provided as deck name in our library. Adding this configuration setting, we can create the library files in the firmware directory. Library files used in decks are located at;
crazyflie-firmware ->> src -> hal -> [interface (.h) / src (.c)]
Create two files, pca9555.h in interface directory and pca9555.c in src directory. Add the following configuration setting in firmware make file that is located in the crazyflie-firmware root directory.
Following is the source code for pca9555.h
PROJ_OBJ_CF2 += pca9555.o
/** * || ____ _ __ * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ * * Crazyflie control firmware * * Copyright (C) 2017 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/>. * * pca9555.h - Functions for interfacing PCA9555 I2C GPIO extender */ #ifndef __PCA9555_H__ #define __PCA9555_H__ #include <stdint.h> #include <stdbool.h> #define PCA9555_DEFAULT_ADDRESS 0b0100000 #define PCA9555_INPUT_REGA (0x00) #define PCA9555_INPUT_REGB (0x01) #define PCA9555_OUTPUT_REGA (0x02) #define PCA9555_OUTPUT_REGB (0x03) #define PCA9555_POL_REGA (0x04) #define PCA9555_POL_REGB (0x05) #define PCA9555_CONFIG_REGA (0x06) #define PCA9555_CONFIG_REGB (0x07) // Belongs to Set A #define PCA9555_P00 (1 << 0) #define PCA9555_P01 (1 << 1) #define PCA9555_P02 (1 << 2) #define PCA9555_P03 (1 << 3) #define PCA9555_P04 (1 << 4) #define PCA9555_P05 (1 << 5) #define PCA9555_P06 (1 << 6) #define PCA9555_P07 (1 << 7) // Belongs to Set B #define PCA9555_P10 (1 << 0) #define PCA9555_P11 (1 << 1) #define PCA9555_P12 (1 << 2) #define PCA9555_P13 (1 << 3) #define PCA9555_P14 (1 << 4) #define PCA9555_P15 (1 << 5) #define PCA9555_P16 (1 << 6) #define PCA9555_P17 (1 << 7) /** * Initialize the PCA9555 sub-system. */ void pca9555Init(); /** * Test the PCA9555 sub-system. */ bool pca9555Test(); /** * Set the output register values. */ bool pca9555ConfigOutputRegA(uint32_t val); bool pca9555ConfigOutputRegB(uint32_t val); /** * Set output bits. */ bool pca9555SetOutputRegA(uint32_t mask); bool pca9555SetOutputRegB(uint32_t mask); /** * Reset output bits. */ bool pca9555ClearOutputRegA(uint32_t mask); bool pca9555ClearOutputRegB(uint32_t mask); /** * Turns LED on to indicate deck has initiated successfully. */ void turnLEDON(); #endif //__PCA9555_H__
Following is the source code for pca9555.c
/** * || ____ _ __ * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ * * Crazyflie control firmware * * Copyright (C) 2017 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/>. * * pca9555.c - Functions for interfacing PCA9555 I2C GPIO extender */ #define DEBUG_MODULE "PCA9555" #include "i2cdev.h" #include "pca9555.h" #include "debug.h" static uint8_t devAddr; static I2C_Dev *I2Cx; void pca9555Init() { i2cdevInit(I2C1_DEV); I2Cx = I2C1_DEV; devAddr = PCA9555_DEFAULT_ADDRESS; } /** * Reads the config registers and checks if there are any errors in reading * the registers */ bool pca9555Test() { uint8_t tb; bool pass_set1, pass_set2; // Test reading the config registers pass_set1 = i2cdevReadByte(I2Cx, devAddr, PCA9555_CONFIG_REGA, &tb); pass_set2 = i2cdevReadByte(I2Cx, devAddr, PCA9555_CONFIG_REGB, &tb); return pass_set1 & pass_set2; } bool pca9555ConfigOutputRegA(uint32_t val) { return i2cdevWriteByte(I2Cx, devAddr, PCA9555_CONFIG_REGA, val); } bool pca9555ConfigOutputRegB(uint32_t val) { return i2cdevWriteByte(I2Cx, devAddr, PCA9555_CONFIG_REGB, val); } bool pca9555SetOutputRegA(uint32_t mask) { uint8_t val; bool pass; pass = i2cdevReadByte(I2Cx, devAddr, PCA9555_OUTPUT_REGA, &val); val |= mask; pass = i2cdevWriteByte(I2Cx, devAddr, PCA9555_OUTPUT_REGA, val); return pass; } bool pca9555SetOutputRegB(uint32_t mask) { uint8_t val; bool pass; pass = i2cdevReadByte(I2Cx, devAddr, PCA9555_OUTPUT_REGB, &val); val |= mask; pass = i2cdevWriteByte(I2Cx, devAddr, PCA9555_OUTPUT_REGB, val); return pass; } bool pca9555ClearOutputRegA(uint32_t mask) { uint8_t val; bool pass; pass = i2cdevReadByte(I2Cx, devAddr, PCA9555_OUTPUT_REGA, &val); val &= ~mask; pass = i2cdevWriteByte(I2Cx, devAddr, PCA9555_OUTPUT_REGA, val); return pass; } bool pca9555ClearOutputRegB(uint32_t mask) { uint8_t val; bool pass; pass = i2cdevReadByte(I2Cx, devAddr, PCA9555_OUTPUT_REGB, &val); val &= ~mask; pass = i2cdevWriteByte(I2Cx, devAddr, PCA9555_OUTPUT_REGB, val); return pass; }
void turnLEDON() { uint8_t val; i2cdevReadByte(I2Cx, devAddr, PCA9555_OUTPUT_REGA, &val); val = val - 1; i2cdevWriteByte(I2Cx, devAddr, PCA9555_OUTPUT_REGA, val); }
Final step is to utilize this newly made library in actual hardware. We will be modifying the existing oa.c file with following content.
/* * || ____ _ __ * +------+ / __ )(_) /_______________ _____ ___ * | 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 "param.h" #define DEBUG_MODULE "OA" #include "system.h" #include "debug.h" #include "log.h" #include "pca9555.h" #include "vl53l1x.h" #include "i2cdev.h" #include "FreeRTOS.h" #include "task.h" #include <stdlib.h> static bool isInit = false; static bool isTested = false; #define OA_PIN_LIGHT PCA9555_P00 #define OA_PIN_NORTH PCA9555_P02 #define OA_PIN_NEAST PCA9555_P03 #define OA_PIN_EFLAT PCA9555_P04 #define OA_PIN_EDOWN PCA9555_P05 #define OA_PIN_ERISE PCA9555_P06 #define OA_PIN_SEAST PCA9555_P07 #define OA_PIN_SOUTH PCA9555_P10 #define OA_PIN_SWEST PCA9555_P11 #define OA_PIN_WRISE PCA9555_P12 #define OA_PIN_WDOWN PCA9555_P13 #define OA_PIN_WFLAT PCA9555_P14 #define OA_PIN_NWEST PCA9555_P01 #define OA_PIN_UPPER PCA9555_P17 static VL53L1_Dev_t devNORTH; // 50 static VL53L1_Dev_t devNEAST; // 51 static VL53L1_Dev_t devEFLAT; // 52 static VL53L1_Dev_t devEDOWN; // 53 static VL53L1_Dev_t devERISE; // 54 static VL53L1_Dev_t devSEAST; // 55 static VL53L1_Dev_t devSOUTH; // 56 static VL53L1_Dev_t devSWEST; // 57 static VL53L1_Dev_t devWRISE; // 58 static VL53L1_Dev_t devWDOWN; // 59 static VL53L1_Dev_t devWFLAT; // 60 static VL53L1_Dev_t devNWEST; // 49 static VL53L1_Dev_t devUPPER; // 61 static uint16_t rNORTH; static uint16_t rNEAST; static uint16_t rEFLAT; static uint16_t rEDOWN; static uint16_t rERISE; static uint16_t rSEAST; static uint16_t rSOUTH; static uint16_t rSWEST; static uint16_t rWRISE; static uint16_t rWDOWN; static uint16_t rWFLAT; static uint16_t rNWEST; static uint16_t rUPPER; static uint16_t oaGetMeasurementAndRestart(VL53L1_Dev_t *dev) { VL53L1_RangingMeasurementData_t rangingData; uint8_t dataReady = 0; uint16_t range; while (dataReady == 0) { VL53L1_GetMeasurementDataReady(dev, &dataReady); vTaskDelay(M2T(1)); } VL53L1_GetRangingMeasurementData(dev, &rangingData); range = rangingData.RangeMilliMeter; VL53L1_StopMeasurement(dev);VL53L1_StartMeasurement(dev); return range; } static void oaTask(void *param) { systemWaitStart(); VL53L1_StopMeasurement(&devNORTH);VL53L1_StartMeasurement(&devNORTH); VL53L1_StopMeasurement(&devNEAST);VL53L1_StartMeasurement(&devNEAST); VL53L1_StopMeasurement(&devEFLAT);VL53L1_StartMeasurement(&devEFLAT); VL53L1_StopMeasurement(&devEDOWN);VL53L1_StartMeasurement(&devEDOWN); VL53L1_StopMeasurement(&devERISE);VL53L1_StartMeasurement(&devERISE); VL53L1_StopMeasurement(&devSEAST);VL53L1_StartMeasurement(&devSEAST); VL53L1_StopMeasurement(&devSOUTH);VL53L1_StartMeasurement(&devSOUTH); VL53L1_StopMeasurement(&devSWEST);VL53L1_StartMeasurement(&devSWEST); VL53L1_StopMeasurement(&devWRISE);VL53L1_StartMeasurement(&devWRISE); VL53L1_StopMeasurement(&devWDOWN);VL53L1_StartMeasurement(&devWDOWN); VL53L1_StopMeasurement(&devWFLAT);VL53L1_StartMeasurement(&devWFLAT); VL53L1_StopMeasurement(&devNWEST);VL53L1_StartMeasurement(&devNWEST); VL53L1_StopMeasurement(&devUPPER);VL53L1_StartMeasurement(&devUPPER); TickType_t lastWakeTime = xTaskGetTickCount(); while (1) { vTaskDelayUntil(&lastWakeTime, M2T(50)); rNORTH = oaGetMeasurementAndRestart(&devNORTH); rNEAST = oaGetMeasurementAndRestart(&devNEAST); rEFLAT = oaGetMeasurementAndRestart(&devEFLAT); rEDOWN = oaGetMeasurementAndRestart(&devEDOWN); rERISE = oaGetMeasurementAndRestart(&devERISE); rSEAST = oaGetMeasurementAndRestart(&devSEAST); rSOUTH = oaGetMeasurementAndRestart(&devSOUTH); rSWEST = oaGetMeasurementAndRestart(&devSWEST); rWRISE = oaGetMeasurementAndRestart(&devWRISE); rWDOWN = oaGetMeasurementAndRestart(&devWDOWN); rWFLAT = oaGetMeasurementAndRestart(&devWFLAT); rNWEST = oaGetMeasurementAndRestart(&devNWEST); rUPPER = oaGetMeasurementAndRestart(&devUPPER); } } static void oaInit() { if (isInit) { return; } // Initiate PCA Driver pca9555Init(); // Output port configuration pca9555ConfigOutputRegA(~( OA_PIN_LIGHT | OA_PIN_NWEST | OA_PIN_NORTH | OA_PIN_NEAST | OA_PIN_EFLAT | OA_PIN_EDOWN | OA_PIN_ERISE | OA_PIN_SEAST )); pca9555ConfigOutputRegB(~( OA_PIN_SOUTH | OA_PIN_SWEST | OA_PIN_WRISE | OA_PIN_WDOWN | OA_PIN_WFLAT | OA_PIN_UPPER )); // Clear output ports pca9555ClearOutputRegA( OA_PIN_LIGHT | OA_PIN_NWEST | OA_PIN_NORTH | OA_PIN_NEAST | OA_PIN_EFLAT | OA_PIN_EDOWN | OA_PIN_ERISE | OA_PIN_SEAST ); pca9555ClearOutputRegB( OA_PIN_SOUTH | OA_PIN_SWEST | OA_PIN_WRISE | OA_PIN_WDOWN | OA_PIN_WFLAT | OA_PIN_UPPER ); isInit = true; DEBUG_PRINT("Initiated FYP bcOA Deck [OK]\n"); xTaskCreate(oaTask, "oa", 2 * configMINIMAL_STACK_SIZE, NULL, 3, NULL); } static bool oaTest() { bool pass = isInit; if (isTested) { DEBUG_PRINT("Cannot test FYP bcOA deck a second time\n"); return false; } pca9555SetOutputRegA(OA_PIN_LIGHT); DEBUG_PRINT("Initiating ToF Setup!\n"); pca9555SetOutputRegA(OA_PIN_NWEST); if (vl53l1xInit(&devNWEST, I2C1_DEV)) { DEBUG_PRINT("Init NWEST sensor [OK]\n"); } else {
DEBUG_PRINT("Init NWEST sensor [FAIL]\n"); pass = false; } pca9555SetOutputRegA(OA_PIN_NORTH); if (vl53l1xInit(&devNORTH, I2C1_DEV)) { DEBUG_PRINT("Init NORTH sensor [OK]\n"); } else {
DEBUG_PRINT("Init NORTH sensor [FAIL]\n"); pass = false; } pca9555SetOutputRegA(OA_PIN_NEAST); if (vl53l1xInit(&devNEAST, I2C1_DEV)) { DEBUG_PRINT("Init NEAST sensor [OK]\n");
} else { DEBUG_PRINT("Init NEAST sensor [FAIL]\n"); pass = false; } pca9555SetOutputRegA(OA_PIN_EFLAT); if (vl53l1xInit(&devEFLAT, I2C1_DEV)) { DEBUG_PRINT("Init EFLAT sensor [OK]\n"); } else { DEBUG_PRINT("Init EFLAT sensor [FAIL]\n"); pass = false; } pca9555SetOutputRegA(OA_PIN_EDOWN); if (vl53l1xInit(&devEDOWN, I2C1_DEV)) {
DEBUG_PRINT("Init EDOWN sensor [OK]\n"); } else { DEBUG_PRINT("Init EDOWN sensor [FAIL]\n"); pass = false; } pca9555SetOutputRegA(OA_PIN_ERISE); if (vl53l1xInit(&devERISE, I2C1_DEV)) { DEBUG_PRINT("Init ERISE sensor [OK]\n"); } else { DEBUG_PRINT("Init ERISE sensor [FAIL]\n"); pass = false; } pca9555SetOutputRegA(OA_PIN_SEAST); if (vl53l1xInit(&devSEAST, I2C1_DEV)) { DEBUG_PRINT("Init SEAST sensor [OK]\n"); } else { DEBUG_PRINT("Init SEAST sensor [FAIL]\n"); pass = false; } pca9555SetOutputRegB(OA_PIN_SOUTH); if (vl53l1xInit(&devSOUTH, I2C1_DEV)) { DEBUG_PRINT("Init SOUTH sensor [OK]\n"); } else { DEBUG_PRINT("Init SOUTH sensor [FAIL]\n"); pass = false; } pca9555SetOutputRegB(OA_PIN_SWEST); if (vl53l1xInit(&devSWEST, I2C1_DEV)) { DEBUG_PRINT("Init SWEST sensor [OK]\n"); } else { DEBUG_PRINT("Init SWEST sensor [FAIL]\n"); pass = false; } pca9555SetOutputRegB(OA_PIN_WRISE); if (vl53l1xInit(&devWRISE, I2C1_DEV)) { DEBUG_PRINT("Init WRISE sensor [OK]\n"); } else { DEBUG_PRINT("Init WRISE sensor [FAIL]\n"); pass = false; } pca9555SetOutputRegB(OA_PIN_WDOWN); if (vl53l1xInit(&devWDOWN, I2C1_DEV)) { DEBUG_PRINT("Init WDOWN sensor [OK]\n"); } else { DEBUG_PRINT("Init WDOWN sensor [FAIL]\n"); pass = false; } pca9555SetOutputRegB(OA_PIN_WFLAT);
if (vl53l1xInit(&devWFLAT, I2C1_DEV)) { DEBUG_PRINT("Init WFLAT sensor [OK]\n"); } else { DEBUG_PRINT("Init WFLAT sensor [FAIL]\n"); pass = false; } pca9555SetOutputRegB(OA_PIN_UPPER); if (vl53l1xInit(&devUPPER, I2C1_DEV)) { DEBUG_PRINT("Init UPPER sensor [OK]\n"); } else { DEBUG_PRINT("Init UPPER sensor [FAIL]\n");
pass = false; } // Turn LED ON turnLEDON(); isTested = true; return pass; } static const DeckDriver oa_deck = { .vid = 0xBC, .pid = 0x0B, .name = "bcOA", .usedGpio = 0, .init = oaInit, .test = oaTest, }; DECK_DRIVER(oa_deck); PARAM_GROUP_START(deck) PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcOA, &isInit) PARAM_GROUP_STOP(deck) LOG_GROUP_START(oa) LOG_ADD(LOG_UINT16, NORTH, &rNORTH) LOG_ADD(LOG_UINT16, NEAST, &rNEAST) LOG_ADD(LOG_UINT16, EFLAT, &rEFLAT) LOG_ADD(LOG_UINT16, EDOWN, &rEDOWN) LOG_ADD(LOG_UINT16, ERISE, &rERISE) LOG_ADD(LOG_UINT16, SEAST, &rSEAST) LOG_ADD(LOG_UINT16, SOUTH, &rSOUTH) LOG_ADD(LOG_UINT16, SWEST, &rSWEST) LOG_ADD(LOG_UINT16, WRISE, &rWRISE) LOG_ADD(LOG_UINT16, WDOWN, &rWDOWN) LOG_ADD(LOG_UINT16, WFLAT, &rWFLAT) LOG_ADD(LOG_UINT16, NWEST, &rNWEST) LOG_ADD(LOG_UINT16, UPPER, &rUPPER) LOG_GROUP_STOP(oa)
Build the complete project to see if the firmware compiles the new files without any errors. Once completed, upload the firmware to crazyflie 2.0 and test readings from every sensor.
Comments
Post a Comment