diff --git a/Makefile b/Makefile index f9c09472c5..a2287591d3 100644 --- a/Makefile +++ b/Makefile @@ -682,8 +682,10 @@ HIGHEND_SRC = \ drivers/rangefinder/rangefinder_hcsr04_i2c.c \ drivers/rangefinder/rangefinder_srf10.c \ drivers/rangefinder/rangefinder_vl53l0x.c \ - drivers/opflow_fake.c \ + drivers/opflow/opflow_fake.c \ + drivers/opflow/opflow_virtual.c \ drivers/vtx_common.c \ + io/opflow_cxof.c \ io/dashboard.c \ io/displayport_max7456.c \ io/displayport_msp.c \ diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 72b9cfc524..bfaf6b11b9 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -54,5 +54,6 @@ typedef enum { DEBUG_RANGEFINDER_QUALITY, DEBUG_PITOT, DEBUG_AGL, + DEBUG_FLOW_RAW, DEBUG_COUNT } debugType_e; diff --git a/src/main/drivers/opflow.h b/src/main/drivers/opflow/opflow.h similarity index 100% rename from src/main/drivers/opflow.h rename to src/main/drivers/opflow/opflow.h diff --git a/src/main/drivers/opflow_fake.c b/src/main/drivers/opflow/opflow_fake.c similarity index 100% rename from src/main/drivers/opflow_fake.c rename to src/main/drivers/opflow/opflow_fake.c diff --git a/src/main/drivers/opflow_fake.h b/src/main/drivers/opflow/opflow_fake.h similarity index 100% rename from src/main/drivers/opflow_fake.h rename to src/main/drivers/opflow/opflow_fake.h diff --git a/src/main/drivers/opflow/opflow_virtual.c b/src/main/drivers/opflow/opflow_virtual.c new file mode 100755 index 0000000000..b09ea78490 --- /dev/null +++ b/src/main/drivers/opflow/opflow_virtual.c @@ -0,0 +1,66 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file 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/. + */ + +/* + * This is a bridge driver between a sensor reader that operates at high level (on top of UART or UIB) + */ + +#include +#include +#include + +#include + +#include "build/build_config.h" + +#include "common/utils.h" + +#include "drivers/opflow/opflow_virtual.h" + +static const virtualOpflowVTable_t * highLevelDeviceVTable = NULL; + +static bool virtualOpflowInit(opflowDev_t * dev) +{ + UNUSED(dev); + return highLevelDeviceVTable->init(); +} + +static bool virtualOpflowUpdate(opflowDev_t * dev) +{ + UNUSED(dev); + return highLevelDeviceVTable->update(&dev->rawData); +} + +bool virtualOpflowDetect(opflowDev_t * dev, const virtualOpflowVTable_t * vtable) +{ + if (vtable && vtable->detect()) { + highLevelDeviceVTable = vtable; + dev->initFn = &virtualOpflowInit; + dev->updateFn = &virtualOpflowUpdate; + memset(&dev->rawData, 0, sizeof(opflowData_t)); + return true; + } + + return false; +} diff --git a/src/main/drivers/opflow/opflow_virtual.h b/src/main/drivers/opflow/opflow_virtual.h new file mode 100755 index 0000000000..5a87f94c7a --- /dev/null +++ b/src/main/drivers/opflow/opflow_virtual.h @@ -0,0 +1,36 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file 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/. + */ + +#pragma once + +#include "drivers/opflow/opflow.h" + +typedef struct virtualOpflowVTable_s { + bool (*detect)(void); + bool (*init)(void); + bool (*update)(opflowData_t * data); +} virtualOpflowVTable_t; + +bool virtualOpflowDetect(opflowDev_t * dev, const virtualOpflowVTable_t * vtable); + diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6f9edfd8a2..67aa119da7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -485,6 +485,22 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #endif break; + case MSP2_INAV_OPTICAL_FLOW: +#ifdef USE_OPTICAL_FLOW + sbufWriteU8(dst, opflow.rawQuality); + sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[X])); + sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[Y])); + sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.bodyRate[X])); + sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.bodyRate[Y])); +#else + sbufWriteU8(dst, opflow.rawQuality); + sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[X])); + sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[Y])); + sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.bodyRate[X])); + sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.bodyRate[Y])); +#endif + break; + case MSP_ANALOG: sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255)); sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index e35969e5b6..86a9d5469f 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -574,7 +574,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_OPFLOW] = { .taskName = "OPFLOW", .taskFunc = taskUpdateOpticalFlow, - .desiredPeriod = TASK_PERIOD_HZ(100), // I2C/SPI sensor will work at higher rate and accumulate, UIB sensor will work at lower rate w/o accumulation + .desiredPeriod = TASK_PERIOD_HZ(100), // I2C/SPI sensor will work at higher rate and accumulate, UIB/UART sensor will work at lower rate w/o accumulation .staticPriority = TASK_PRIORITY_MEDIUM, }, #endif diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5705206454..c6c87185f5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -13,7 +13,7 @@ tables: values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "FAKE"] enum: magSensor_e - name: opflow_hardware - values: ["NONE", "FAKE"] + values: ["NONE", "FAKE", "CXOF", "PMW3901"] enum: opticalFlowSensor_e - name: baro_hardware values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "FAKE"] @@ -66,7 +66,7 @@ tables: - name: i2c_speed values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"] - name: debug_modes - values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "RFIND", "RFIND_Q", "PITOT", "AGL"] + values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "RFIND", "RFIND_Q", "PITOT", "AGL", "FLOW_RAW"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator diff --git a/src/main/io/opflow.h b/src/main/io/opflow.h new file mode 100755 index 0000000000..a2bd4733b9 --- /dev/null +++ b/src/main/io/opflow.h @@ -0,0 +1,32 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file 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/. + */ + +#pragma once + +#include + +#include "sensors/opflow.h" +#include "drivers/opflow/opflow_virtual.h" + +extern virtualOpflowVTable_t opflowCxofVtable; \ No newline at end of file diff --git a/src/main/io/opflow_cxof.c b/src/main/io/opflow_cxof.c new file mode 100755 index 0000000000..52f95107b5 --- /dev/null +++ b/src/main/io/opflow_cxof.c @@ -0,0 +1,145 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file 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/. + */ + +#include +#include +#include +#include + +#include "platform.h" + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/maths.h" + +#include "io/serial.h" + +#if defined(USE_OPFLOW_CXOF) +#include "drivers/opflow/opflow_virtual.h" +#include "drivers/time.h" +#include "io/opflow.h" + +#define CXOF_PACKET_SIZE 9 +static serialPort_t * flowPort = NULL; +static serialPortConfig_t * portConfig; +static uint8_t buffer[10]; +static int bufferPtr; + +typedef struct __attribute__((packed)) { + uint8_t header; + uint8_t res0; + int16_t motionX; + int16_t motionY; + int8_t motionT; + uint8_t squal; + uint8_t footer; +} cxofPacket_t; + +static bool cxofOpflowDetect(void) +{ + portConfig = findSerialPortConfig(FUNCTION_OPTICAL_FLOW); + if (!portConfig) { + return false; + } + + return true; +} + +static bool cxofOpflowInit(void) +{ + if (!portConfig) { + return false; + } + + flowPort = openSerialPort(portConfig->identifier, FUNCTION_OPTICAL_FLOW, NULL, NULL, baudRates[BAUD_19200], MODE_RX, SERIAL_NOT_INVERTED); + if (!flowPort) { + return false; + } + + bufferPtr = 0; + + return true; +} + +static bool cxofOpflowUpdate(opflowData_t * data) +{ + static timeUs_t previousTimeUs = 0; + const timeUs_t currentTimeUs = micros(); + + bool newPacket = false; + opflowData_t tmpData = { 0 }; + + while (serialRxBytesWaiting(flowPort) > 0) { + uint8_t c = serialRead(flowPort); + + // Wait for header + if (bufferPtr == 0) { + if (c != 0xFE) { + break; + } + } + + // Consume received byte + if (bufferPtr < CXOF_PACKET_SIZE) { + buffer[bufferPtr++] = c; + + if (bufferPtr == CXOF_PACKET_SIZE) { + cxofPacket_t * pkt = (cxofPacket_t *)&buffer[0]; + + if (pkt->header == 0xFE && pkt->footer == 0xAA) { + // Valid packet + tmpData.deltaTime += (currentTimeUs - previousTimeUs); + tmpData.flowRateRaw[0] += pkt->motionX; + tmpData.flowRateRaw[1] += -pkt->motionY; // Flow sensor is facing down, apply 180 roll rotation + tmpData.quality = (constrain(pkt->squal, 64, 78) - 64) * 100 / 14; + + previousTimeUs = currentTimeUs; + newPacket = true; + } + + // Reset the decoder + bufferPtr = 0; + } + } + else { + // In case of buffer overflow - reset the decoder + bufferPtr = 0; + } + } + + if (newPacket) { + *data = tmpData; + } + + return newPacket; +} + +virtualOpflowVTable_t opflowCxofVtable = { + .detect = cxofOpflowDetect, + .init = cxofOpflowInit, + .update = cxofOpflowUpdate +}; + +#endif \ No newline at end of file diff --git a/src/main/io/serial.h b/src/main/io/serial.h index a971e25011..65c2eaea51 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -45,6 +45,7 @@ typedef enum { FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048 FUNCTION_VTX_TRAMP = (1 << 12), // 4096 FUNCTION_UAV_INTERCONNECT = (1 << 13), // 8192 + FUNCTION_OPTICAL_FLOW = (1 << 14), // 16384 } serialPortFunction_e; typedef enum { diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 17d4c8031c..26e105d5b8 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -19,3 +19,4 @@ // See https://github.com/iNavFlight/inav/wiki/MSP-V2#msp-v2-message-catalogue #define MSP2_INAV_STATUS 0x2000 +#define MSP2_INAV_OPTICAL_FLOW 0x2001 \ No newline at end of file diff --git a/src/main/sensors/opflow.c b/src/main/sensors/opflow.c index 8e0ad7f035..0a7ed79598 100755 --- a/src/main/sensors/opflow.c +++ b/src/main/sensors/opflow.c @@ -43,18 +43,22 @@ #include "drivers/io.h" #include "drivers/logging.h" #include "drivers/time.h" -#include "sensors/gyro.h" -#include "drivers/opflow.h" -#include "drivers/opflow_fake.h" + +#include "drivers/opflow/opflow.h" +#include "drivers/opflow/opflow_fake.h" +#include "drivers/opflow/opflow_virtual.h" #include "fc/config.h" #include "fc/runtime_config.h" +#include "sensors/gyro.h" #include "sensors/sensors.h" #include "sensors/opflow.h" #include "scheduler/scheduler.h" +#include "io/opflow.h" + #include "build/debug.h" opflow_t opflow; @@ -85,6 +89,14 @@ static bool opflowDetect(opflowDev_t * dev, uint8_t opflowHardwareToUse) #endif break; + case OPFLOW_CXOF: +#if defined(USE_OPFLOW_CXOF) + if (virtualOpflowDetect(dev, &opflowCxofVtable)) { // FIXME: Do actual detection if HC-SR04 is plugged in + opflowHardware = OPFLOW_CXOF; + } +#endif + break; + case OPFLOW_NONE: opflowHardware = OPFLOW_NONE; break; @@ -115,7 +127,11 @@ bool opflowInit(void) return false; } - opflow.dev.initFn(&opflow.dev); + if (!opflow.dev.initFn(&opflow.dev)) { + sensorsClear(SENSOR_OPFLOW); + return false; + } + opflowZeroBodyGyroAcc(); return true; @@ -133,6 +149,7 @@ void opflowUpdate(timeUs_t currentTimeUs) // Indicate valid update opflow.lastValidUpdate = currentTimeUs; opflow.isHwHealty = true; + opflow.rawQuality = opflow.dev.rawData.quality; // Handle state switching switch (opflow.flowQuality) { @@ -156,6 +173,11 @@ void opflowUpdate(timeUs_t currentTimeUs) opflow.bodyRate[X] = DEGREES_TO_RADIANS(opflow.gyroBodyRateAcc[X] / opflow.gyroBodyRateTimeUs); opflow.bodyRate[Y] = DEGREES_TO_RADIANS(opflow.gyroBodyRateAcc[Y] / opflow.gyroBodyRateTimeUs); + + DEBUG_SET(DEBUG_FLOW_RAW, 0, RADIANS_TO_DEGREES(opflow.flowRate[X])); + DEBUG_SET(DEBUG_FLOW_RAW, 1, RADIANS_TO_DEGREES(opflow.flowRate[Y])); + DEBUG_SET(DEBUG_FLOW_RAW, 2, RADIANS_TO_DEGREES(opflow.bodyRate[X])); + DEBUG_SET(DEBUG_FLOW_RAW, 3, RADIANS_TO_DEGREES(opflow.bodyRate[Y])); } else { // Opflow updated but invalid - zero out flow rates and body @@ -175,6 +197,7 @@ void opflowUpdate(timeUs_t currentTimeUs) opflow.isHwHealty = false; opflow.flowQuality = OPFLOW_QUALITY_INVALID; + opflow.rawQuality = 0; opflow.flowRate[X] = 0; opflow.flowRate[Y] = 0; diff --git a/src/main/sensors/opflow.h b/src/main/sensors/opflow.h index eaa8e077ff..dcfb4b4874 100755 --- a/src/main/sensors/opflow.h +++ b/src/main/sensors/opflow.h @@ -26,11 +26,13 @@ #include #include "config/parameter_group.h" -#include "drivers/opflow.h" +#include "drivers/opflow/opflow.h" typedef enum { OPFLOW_NONE = 0, OPFLOW_FAKE = 1, + OPFLOW_CXOF = 2, + OPFLOW_PMW3901 = 3 } opticalFlowSensor_e; typedef enum { @@ -56,6 +58,8 @@ typedef struct opflow_s { float gyroBodyRateAcc[2]; timeUs_t gyroBodyRateTimeUs; + + uint8_t rawQuality; } opflow_t; extern opflow_t opflow; diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 228ef32b6e..ab24267e67 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -67,8 +67,8 @@ //#define USE_PITOT_MS4525 //#define PITOT_I2C_BUS BUS_I2C2 -// #define USE_OPTICAL_FLOW -// #define USE_OPFLOW_FAKE +#define USE_OPTICAL_FLOW +#define USE_OPFLOW_CXOF #define USE_RANGEFINDER #define USE_RANGEFINDER_UIB diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 9ddfdc2244..9a20d241d4 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -79,7 +79,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 #define UART3_RX_PIN PB11 @@ -111,25 +110,20 @@ #define I2C_DEVICE_2_SHARES_UART3 #define USE_ADC -// PC2 shared with HC-SR04 #define ADC_CHANNEL_1_PIN PC2 #define ADC_CHANNEL_2_PIN PC1 #define VBAT_ADC_CHANNEL ADC_CHN_2 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 -#define CURRENT_METER_ADC_PIN PC2 -#define VBAT_ADC_PIN PC1 - -#define USE_LED_STRIP -#define LED_STRIP_TIMER TIM5 +#define LED_STRIP +#define LED_STRIP_TIMER TIM5 #define USE_RANGEFINDER #define USE_RANGEFINDER_VL53L0X -#define VL53L0X_I2C_BUS BUS_I2C2 -// #define USE_RANGEFINDER_HCSR04 -// #define RANGEFINDER_HCSR04_TRIGGER_PIN PC2 -// #define RANGEFINDER_HCSR04_ECHO_PIN PC3 -// #define USE_RANGEFINDER_SRF10 +#define VL53L0X_I2C_BUS BUS_I2C2 + +#define USE_OPTICAL_FLOW +#define USE_OPFLOW_CXOF #define DEFAULT_FEATURES FEATURE_BLACKBOX #define DEFAULT_RX_TYPE RX_TYPE_SERIAL