diff --git a/mk/source.mk b/mk/source.mk index 317d4c4355..70e4d00b57 100644 --- a/mk/source.mk +++ b/mk/source.mk @@ -252,6 +252,7 @@ COMMON_SRC = \ osd/osd_warnings.c \ sensors/barometer.c \ sensors/rangefinder.c \ + sensors/opticalflow.c \ telemetry/telemetry.c \ telemetry/crsf.c \ telemetry/ghst.c \ diff --git a/src/main/build/debug.c b/src/main/build/debug.c index f51ea8a4a1..3d67bae371 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -55,6 +55,7 @@ const char * const debugModeNames[DEBUG_COUNT] = { [DEBUG_FPORT] = "FPORT", [DEBUG_RANGEFINDER] = "RANGEFINDER", [DEBUG_RANGEFINDER_QUALITY] = "RANGEFINDER_QUALITY", + [DEBUG_OPTICALFLOW] = "OPTICALFLOW", [DEBUG_LIDAR_TF] = "LIDAR_TF", [DEBUG_ADC_INTERNAL] = "ADC_INTERNAL", [DEBUG_RUNAWAY_TAKEOFF] = "RUNAWAY_TAKEOFF", diff --git a/src/main/build/debug.h b/src/main/build/debug.h index b0070b69de..6e6e43a67a 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -57,6 +57,7 @@ typedef enum { DEBUG_FPORT, DEBUG_RANGEFINDER, DEBUG_RANGEFINDER_QUALITY, + DEBUG_OPTICALFLOW, DEBUG_LIDAR_TF, DEBUG_ADC_INTERNAL, DEBUG_RUNAWAY_TAKEOFF, diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 36ea8155e8..926b5ada5a 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -239,6 +239,7 @@ static const char * const featureNames[] = { _R(FEATURE_SOFTSERIAL, "SOFTSERIAL"), _R(FEATURE_GPS, "GPS"), _R(FEATURE_RANGEFINDER, "RANGEFINDER"), + _R(FEATURE_OPTICALFLOW, "OPTICALFLOW"), _R(FEATURE_TELEMETRY, "TELEMETRY"), _R(FEATURE_3D, "3D"), _R(FEATURE_RX_PARALLEL_PWM, "RX_PARALLEL_PWM"), @@ -272,7 +273,7 @@ static const char *const sensorTypeNames[] = { #define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG | SENSOR_RANGEFINDER) static const char * const *sensorHardwareNames[] = { - lookupTableGyroHardware, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware, lookupTableRangefinderHardware + lookupTableGyroHardware, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware, lookupTableRangefinderHardware, lookupTableOpticalflowHardware }; #endif // USE_SENSOR_NAMES diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index f01c942fa2..fc4e9a703a 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -129,6 +129,7 @@ #include "sensors/esc_sensor.h" #include "sensors/gyro.h" #include "sensors/rangefinder.h" +#include "sensors/opticalflow.h" #include "scheduler/scheduler.h" @@ -170,6 +171,11 @@ const char * const lookupTableRangefinderHardware[] = { "NONE", "HCSR04", "TFMINI", "TF02", "MTF01", "MTF02", "MTF01P", "MTF02P" }; #endif +#if defined(USE_SENSOR_NAMES) || defined(USE_OPTICALFLOW) +const char * const lookupTableOpticalflowHardware[] = { + "NONE", "MT" +}; +#endif const char * const lookupTableOffOn[] = { "OFF", "ON" @@ -613,6 +619,9 @@ const lookupTableEntry_t lookupTables[] = { #ifdef USE_RANGEFINDER LOOKUP_TABLE_ENTRY(lookupTableRangefinderHardware), #endif +#ifdef USE_OPTICALFLOW + LOOKUP_TABLE_ENTRY(lookupTableOpticalflowHardware), +#endif #ifdef USE_GYRO_OVERFLOW_CHECK LOOKUP_TABLE_ENTRY(lookupTableGyroOverflowCheck), #endif @@ -1710,6 +1719,14 @@ const clivalue_t valueTable[] = { { "rangefinder_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RANGEFINDER_HARDWARE }, PG_RANGEFINDER_CONFIG, offsetof(rangefinderConfig_t, rangefinder_hardware) }, #endif +// PG_OPTICALFLOW_CONFIG +#ifdef USE_OPTICALFLOW + { "opticalflow_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OPTICALFLOW_HARDWARE }, PG_OPTICALFLOW_CONFIG, offsetof(opticalflowConfig_t, opticalflow_hardware) }, + { "opticalflow_rotation", VAR_INT16 | MASTER_VALUE , .config.minmaxUnsigned = {0, 359}, PG_OPTICALFLOW_CONFIG, offsetof(opticalflowConfig_t, rotation) }, + { "opticalflow_lpf", VAR_UINT16 | MASTER_VALUE , .config.minmaxUnsigned = {0, 10000}, PG_OPTICALFLOW_CONFIG, offsetof(opticalflowConfig_t, flow_lpf) }, + { "opticalflow_flip_x", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OPTICALFLOW_CONFIG, offsetof(opticalflowConfig_t, flip_x) }, +#endif + // PG_PINIO_CONFIG #ifdef USE_PINIO { "pinio_config", VAR_UINT8 | HARDWARE_VALUE | MODE_ARRAY, .config.array.length = PINIO_COUNT, PG_PINIO_CONFIG, offsetof(pinioConfig_t, config) }, diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h index b67664a0e8..b3222f8b94 100644 --- a/src/main/cli/settings.h +++ b/src/main/cli/settings.h @@ -82,6 +82,9 @@ typedef enum { #ifdef USE_RANGEFINDER TABLE_RANGEFINDER_HARDWARE, #endif +#ifdef USE_OPTICALFLOW + TABLE_OPTICALFLOW_HARDWARE, +#endif #ifdef USE_GYRO_OVERFLOW_CHECK TABLE_GYRO_OVERFLOW_CHECK, #endif @@ -260,6 +263,8 @@ extern const char * const lookupTableMagHardware[]; extern const char * const lookupTableRangefinderHardware[]; +extern const char * const lookupTableOpticalflowHardware[]; + extern const char * const lookupTableLedstripColors[]; extern const char * const lookupTableRescueAltitudeMode[]; diff --git a/src/main/config/feature.c b/src/main/config/feature.c index d8e2ebd57f..6dbd490adc 100644 --- a/src/main/config/feature.c +++ b/src/main/config/feature.c @@ -57,6 +57,9 @@ uint32_t featuresSupportedByBuild = #ifdef USE_RANGEFINDER | FEATURE_RANGEFINDER #endif +#ifdef USE_OPTICALFLOW + | FEATURE_OPTICALFLOW +#endif #ifdef USE_TELEMETRY | FEATURE_TELEMETRY #endif diff --git a/src/main/config/feature.h b/src/main/config/feature.h index 039a49e83d..28cec3aa3d 100644 --- a/src/main/config/feature.h +++ b/src/main/config/feature.h @@ -50,6 +50,7 @@ typedef enum { FEATURE_SERVO_TILT = 1 << 5, FEATURE_SOFTSERIAL = 1 << 6, FEATURE_GPS = 1 << 7, + FEATURE_OPTICALFLOW = 1 << 8, FEATURE_RANGEFINDER = 1 << 9, FEATURE_TELEMETRY = 1 << 10, FEATURE_3D = 1 << 12, diff --git a/src/main/drivers/opticalflow/opticalflow.h b/src/main/drivers/opticalflow/opticalflow.h new file mode 100644 index 0000000000..8f3ad6dd29 --- /dev/null +++ b/src/main/drivers/opticalflow/opticalflow.h @@ -0,0 +1,52 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software 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. + * + * Betaflight 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 software. + * + * If not, see . + */ + +#pragma once + +#include +#include "common/vector.h" + +#define OPTICALFLOW_OUT_OF_RANGE -1 +#define OPTICALFLOW_HARDWARE_FAILURE -2 +#define OPTICALFLOW_NO_NEW_DATA -3 + +#define OPTICALFLOW_HARDWARE_TIMEOUT_US 100000 // 100ms + +typedef struct opticalflowData_s { + uint32_t timeStampUs; + int16_t quality; + vector2_t flowRate; +} opticalflowData_t; + +struct opticalflowDev_s; +typedef void opflowOpInitFunc(struct opticalflowDev_s * dev); +typedef void opflowOpUpdateFunc(struct opticalflowDev_s * dev); +typedef void opflowOpReadFunc(struct opticalflowDev_s * dev, opticalflowData_t * result); + +typedef struct opticalflowDev_s { + unsigned delayMs; + int16_t minRangeCm; + uint8_t minQualityThreshold; + + opflowOpInitFunc *init; + opflowOpUpdateFunc *update; + opflowOpReadFunc *read; +} opticalflowDev_t; diff --git a/src/main/drivers/rangefinder/rangefinder_lidarmt.c b/src/main/drivers/rangefinder/rangefinder_lidarmt.c index 863952385c..8204547d1a 100644 --- a/src/main/drivers/rangefinder/rangefinder_lidarmt.c +++ b/src/main/drivers/rangefinder/rangefinder_lidarmt.c @@ -24,7 +24,6 @@ #include #include -#include #include "platform.h" @@ -34,11 +33,21 @@ #include "common/utils.h" +#include "drivers/time.h" + #include "drivers/rangefinder/rangefinder_lidarmt.h" #include "sensors/rangefinder.h" -static bool hasNewData = false; -static int32_t sensorData = RANGEFINDER_NO_NEW_DATA; +#include "drivers/opticalflow/opticalflow.h" + +#define MT_OPTICALFLOW_MIN_RANGE 80 // mm +#define MT_OPFLOW_MIN_QUALITY_THRESHOLD 30 + +static opticalflowData_t opticalflowSensorData = {0}; + +static bool hasRFNewData = false; +static mtRangefinderData_t rfSensorData = {RANGEFINDER_NO_NEW_DATA, 0}; +static const MTRangefinderConfig * deviceConf = NULL; // Initialize the table with values for each rangefinder type static const MTRangefinderConfig rangefinderConfigs[] = { @@ -63,40 +72,41 @@ static void mtRangefinderUpdate(rangefinderDev_t * dev) { static int32_t mtRangefinderGetDistance(rangefinderDev_t * dev) { UNUSED(dev); - if (hasNewData) { - hasNewData = false; - return (sensorData >= 0.0f) ? sensorData : RANGEFINDER_OUT_OF_RANGE; + if (hasRFNewData) { + hasRFNewData = false; + return (rfSensorData.distanceMm >= 0) ? (rfSensorData.distanceMm / 10) : RANGEFINDER_OUT_OF_RANGE; } else { return RANGEFINDER_NO_NEW_DATA; } } bool mtRangefinderDetect(rangefinderDev_t * dev, rangefinderType_e mtRangefinderToUse) { - const MTRangefinderConfig* deviceConf = getMTRangefinderDeviceConf(mtRangefinderToUse); + deviceConf = getMTRangefinderDeviceConf(mtRangefinderToUse); if (!deviceConf) { return false; } + dev->delayMs = deviceConf->delayMs; dev->maxRangeCm = deviceConf->maxRangeCm; dev->detectionConeDeciDegrees = RANGEFINDER_MT_DETECTION_CONE_DECIDEGREES; dev->detectionConeExtendedDeciDegrees = RANGEFINDER_MT_DETECTION_CONE_DECIDEGREES; - dev->init = &mtRangefinderInit; + dev->init = &mtRangefinderInit; dev->update = &mtRangefinderUpdate; - dev->read = &mtRangefinderGetDistance; - + dev->read = &mtRangefinderGetDistance; return true; } void mtRangefinderReceiveNewData(const uint8_t * bufferPtr) { const mspSensorRangefinderLidarMtDataMessage_t * pkt = (const mspSensorRangefinderLidarMtDataMessage_t *)bufferPtr; - sensorData = pkt->distanceMm / 10; - hasNewData = true; + rfSensorData.distanceMm = pkt->distanceMm; + rfSensorData.timestampUs = micros(); + hasRFNewData = true; } -const MTRangefinderConfig* getMTRangefinderDeviceConf(rangefinderType_e mtRangefinderToUse){ +const MTRangefinderConfig* getMTRangefinderDeviceConf(rangefinderType_e mtRangefinderToUse) { for (const MTRangefinderConfig* cfg = rangefinderConfigs; cfg < ARRAYEND(rangefinderConfigs); cfg++) { if (cfg->deviceType == mtRangefinderToUse) { return cfg; @@ -105,4 +115,60 @@ const MTRangefinderConfig* getMTRangefinderDeviceConf(rangefinderType_e mtRangef return NULL; } +const mtRangefinderData_t * getMTRangefinderData(void) { + return &rfSensorData; +} + +typedef struct __attribute__((packed)) { + uint8_t quality; // [0;255] + int32_t motionX; + int32_t motionY; +} mtOpticalflowDataMessage_t; + +static void mtOpticalflowInit(opticalflowDev_t * dev) { + UNUSED(dev); +} + +static void mtOpticalflowUpdate(opticalflowDev_t * dev) { + UNUSED(dev); +} + +static void mtOpticalflowGetData(opticalflowDev_t * dev, opticalflowData_t * result) { + UNUSED(dev); + *result = opticalflowSensorData; +} + +bool mtOpticalflowDetect(opticalflowDev_t * dev, rangefinderType_e mtRangefinderToUse) { + deviceConf = getMTRangefinderDeviceConf(mtRangefinderToUse); + if (!deviceConf) { + return false; + } + + dev->delayMs = deviceConf->delayMs; + dev->minRangeCm = MT_OPTICALFLOW_MIN_RANGE; + dev->minQualityThreshold = MT_OPFLOW_MIN_QUALITY_THRESHOLD; + + dev->init = &mtOpticalflowInit; + dev->update = &mtOpticalflowUpdate; + dev->read = &mtOpticalflowGetData; + + return true; +} + +void mtOpticalflowReceiveNewData(const uint8_t * bufferPtr) { + const mtRangefinderData_t * latestRangefinderData = getMTRangefinderData(); + + const mtOpticalflowDataMessage_t * pkt = (const mtOpticalflowDataMessage_t *)bufferPtr; + + opticalflowSensorData.timeStampUs = micros(); + opticalflowSensorData.flowRate.x = (float)pkt->motionX / 1000.0f; + opticalflowSensorData.flowRate.y = (float)pkt->motionY / 1000.0f; + opticalflowSensorData.quality = pkt->quality * 100 / 255; + + if (latestRangefinderData->distanceMm < MT_OPTICALFLOW_MIN_RANGE) { + opticalflowSensorData.quality = OPTICALFLOW_OUT_OF_RANGE; + } else if (cmp32(micros(), latestRangefinderData->timestampUs) > (5000 * deviceConf->delayMs)) { // 5 updates missing + opticalflowSensorData.quality = OPTICALFLOW_HARDWARE_FAILURE; + } +} #endif // USE_RANGEFINDER_MT diff --git a/src/main/drivers/rangefinder/rangefinder_lidarmt.h b/src/main/drivers/rangefinder/rangefinder_lidarmt.h index 773c61b867..855e58f7cc 100644 --- a/src/main/drivers/rangefinder/rangefinder_lidarmt.h +++ b/src/main/drivers/rangefinder/rangefinder_lidarmt.h @@ -21,6 +21,7 @@ #pragma once #include "drivers/rangefinder/rangefinder.h" +#include "drivers/opticalflow/opticalflow.h" #include "sensors/rangefinder.h" #define RANGEFINDER_MT_DETECTION_CONE_DECIDEGREES 900 @@ -31,6 +32,14 @@ typedef struct { uint16_t maxRangeCm; } MTRangefinderConfig; +typedef struct { + uint32_t timestampUs; + int32_t distanceMm; +} mtRangefinderData_t; + bool mtRangefinderDetect(rangefinderDev_t * dev, rangefinderType_e mtRangefinderToUse); void mtRangefinderReceiveNewData(const uint8_t * bufferPtr); const MTRangefinderConfig* getMTRangefinderDeviceConf(rangefinderType_e mtRangefinderToUse); + +bool mtOpticalflowDetect(opticalflowDev_t * dev, rangefinderType_e mtRangefinderToUse); +void mtOpticalflowReceiveNewData(const uint8_t * bufferPtr); diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 0317cf537f..62885df0fa 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -98,6 +98,7 @@ #include "sensors/gyro.h" #include "sensors/sensors.h" #include "sensors/rangefinder.h" +#include "sensors/opticalflow.h" #include "telemetry/telemetry.h" #include "telemetry/crsf.h" @@ -302,6 +303,20 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs) } #endif +#ifdef USE_OPTICALFLOW +void taskUpdateOpticalflow(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + if (!sensors(SENSOR_OPTICALFLOW)) { + return; + } + + opticalflowUpdate(); + opticalflowProcess(); +} +#endif + #ifdef USE_TELEMETRY static void taskTelemetry(timeUs_t currentTimeUs) { @@ -449,7 +464,9 @@ task_attribute_t task_attributes[TASK_COUNT] = { #ifdef USE_RANGEFINDER [TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, taskUpdateRangefinder, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOWEST), #endif - +#ifdef USE_OPTICALFLOW + [TASK_OPTICALFLOW] = DEFINE_TASK("OPTICALFLOW", NULL, NULL, taskUpdateOpticalflow, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOWEST), +#endif #ifdef USE_CRSF_V3 [TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, speedNegotiationProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW), #endif @@ -528,6 +545,12 @@ void tasksInit(void) } #endif +#ifdef USE_OPTICALFLOW + if (sensors(SENSOR_OPTICALFLOW)) { + setTaskEnabled(TASK_OPTICALFLOW, featureIsEnabled(FEATURE_OPTICALFLOW)); + } +#endif + setTaskEnabled(TASK_RX, true); setTaskEnabled(TASK_DISPATCH, dispatchIsEnabled()); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 56fe357f70..dcb6df0f75 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -149,6 +149,7 @@ #include "sensors/gyro.h" #include "sensors/gyro_init.h" #include "sensors/rangefinder.h" +#include "sensors/opticalflow.h" #include "telemetry/msp_shared.h" #include "telemetry/telemetry.h" @@ -2054,7 +2055,7 @@ case MSP_NAME: break; case MSP_SENSOR_CONFIG: - // use sensorIndex_e index: 0:GyroHardware, 1:AccHardware, 2:BaroHardware, 3:MagHardware, 4:RangefinderHardware + // use sensorIndex_e index: 0:GyroHardware, 1:AccHardware, 2:BaroHardware, 3:MagHardware, 4:RangefinderHardware 5:OpticalflowHardware #if defined(USE_ACC) sbufWriteU8(dst, accelerometerConfig()->acc_hardware); #else @@ -2076,6 +2077,12 @@ case MSP_NAME: #else sbufWriteU8(dst, RANGEFINDER_NONE); #endif + +#ifdef USE_OPTICALFLOW + sbufWriteU8(dst, opticalflowConfig()->opticalflow_hardware); +#else + sbufWriteU8(dst, OPTICALFLOW_NONE); +#endif break; // Added in MSP API 1.46 @@ -2107,6 +2114,11 @@ case MSP_NAME: sbufWriteU8(dst, detectedSensors[SENSOR_INDEX_RANGEFINDER]); #else sbufWriteU8(dst, SENSOR_NOT_AVAILABLE); +#endif +#ifdef USE_OPTICAL_FLOW + sbufWriteU8(dst, detectedSensors[SENSOR_INDEX_OPTICALFLOW]); +#else + sbufWriteU8(dst, SENSOR_NOT_AVAILABLE); #endif break; @@ -3301,7 +3313,13 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, #ifdef USE_RANGEFINDER rangefinderConfigMutable()->rangefinder_hardware = sbufReadU8(src); #else - sbufReadU8(src); // rangefinder hardware + sbufReadU8(src); +#endif + +#ifdef USE_OPTICALFLOW + opticalflowConfigMutable()->opticalflow_hardware = sbufReadU8(src); +#else + sbufReadU8(src); #endif break; #ifdef USE_ACC @@ -3658,7 +3676,12 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, case MSP2_SENSOR_RANGEFINDER_LIDARMT: mtRangefinderReceiveNewData(sbufPtr(src)); break; + + case MSP2_SENSOR_OPTICALFLOW_MT: + mtOpticalflowReceiveNewData(sbufPtr(src)); + break; #endif + #ifdef USE_GPS case MSP2_SENSOR_GPS: (void)sbufReadU8(src); // instance diff --git a/src/main/msp/msp_protocol_v2_betaflight.h b/src/main/msp/msp_protocol_v2_betaflight.h index b039defb71..f96cd6a817 100644 --- a/src/main/msp/msp_protocol_v2_betaflight.h +++ b/src/main/msp/msp_protocol_v2_betaflight.h @@ -29,6 +29,7 @@ #define MSP2_GET_LED_STRIP_CONFIG_VALUES 0x3008 #define MSP2_SET_LED_STRIP_CONFIG_VALUES 0x3009 #define MSP2_SENSOR_CONFIG_ACTIVE 0x300A +#define MSP2_SENSOR_OPTICALFLOW 0x300B // MSP2_SET_TEXT and MSP2_GET_TEXT variable types #define MSP2TEXT_PILOT_NAME 1 diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h index be3344c324..28e1469155 100644 --- a/src/main/msp/msp_protocol_v2_common.h +++ b/src/main/msp/msp_protocol_v2_common.h @@ -25,3 +25,4 @@ #define MSP2_SENSOR_GPS 0x1F03 // TODO: implement new, extensible rangefinder protocol #define MSP2_SENSOR_RANGEFINDER_LIDARMT 0x1F01 +#define MSP2_SENSOR_OPTICALFLOW_MT 0x1F02 diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h index caa85f0f30..97e3d125d9 100644 --- a/src/main/pg/pg_ids.h +++ b/src/main/pg/pg_ids.h @@ -158,7 +158,9 @@ #define PG_MSP_CONFIG 557 //#define PG_SOFTSERIAL_PIN_CONFIG 558 // removed, merged into SERIAL_PIN_CONFIG #define PG_GIMBAL_TRACK_CONFIG 559 -#define PG_BETAFLIGHT_END 559 +#define PG_OPTICALFLOW_CONFIG 560 +#define PG_BETAFLIGHT_END 560 + // OSD configuration (subject to change) #define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 88ae1091aa..9e190994cc 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -134,6 +134,9 @@ typedef enum { #ifdef USE_RANGEFINDER TASK_RANGEFINDER, #endif +#ifdef USE_OPTICALFLOW + TASK_OPTICALFLOW, +#endif #if defined(USE_BARO) || defined(USE_GPS) TASK_ALTITUDE, #endif diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 99b9b74e61..f128df173b 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -45,10 +45,11 @@ #include "sensors/initialisation.h" #include "sensors/rangefinder.h" #include "sensors/sensors.h" +#include "sensors/opticalflow.h" // requestedSensors is not actually used -uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE }; -uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE }; +uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, OPTICALFLOW_NONE}; +uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, OPTICALFLOW_NONE}; void sensorsPreInit(void) { @@ -88,6 +89,10 @@ bool sensorsAutodetect(void) rangefinderInit(); #endif +#ifdef USE_OPTICALFLOW + opticalflowInit(); +#endif + #ifdef USE_ADC_INTERNAL adcInternalInit(); #endif diff --git a/src/main/sensors/opticalflow.c b/src/main/sensors/opticalflow.c new file mode 100644 index 0000000000..37f45161a7 --- /dev/null +++ b/src/main/sensors/opticalflow.c @@ -0,0 +1,196 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software 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. + * + * Betaflight 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 software. + * + * If not, see . + */ + + +#include +#include + +#include "platform.h" + +#ifdef USE_OPTICALFLOW + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/maths.h" +#include "common/time.h" +#include "common/utils.h" +#include "common/filter.h" + +#include "config/config.h" +#include "config/feature.h" + +#include "fc/runtime_config.h" + +#include "scheduler/scheduler.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "drivers/time.h" +#include "drivers/rangefinder/rangefinder.h" +#include "drivers/rangefinder/rangefinder_lidarmt.h" + +#include "io/beeper.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" +#include "sensors/opticalflow.h" + +#define OPTICALFLOW_CALIBRATION_DURATION_MS 30000 +#define RATE_SCALE_RESOLUTION (1000.0f) + +// static prototypes +static void applySensorRotation(vector2_t * dst, vector2_t * src); +static void applyLPF(vector2_t * flowRates); + +PG_REGISTER_WITH_RESET_TEMPLATE(opticalflowConfig_t, opticalflowConfig, PG_OPTICALFLOW_CONFIG, 0); + +PG_RESET_TEMPLATE(opticalflowConfig_t, opticalflowConfig, + .opticalflow_hardware = OPTICALFLOW_NONE, + .rotation = 0, + .flow_lpf = 0, + .flip_x = 0 +); + +static opticalflow_t opticalflow; +float cosRotAngle = 1.0f; +float sinRotAngle = 0.0f; +static pt2Filter_t xFlowLpf, yFlowLpf; + +// ====================================================================== +// =================== Opticalflow Main Functions ======================= +// ====================================================================== +static bool opticalflowDetect(opticalflowDev_t * dev, uint8_t opticalflowHardwareToUse) { + UNUSED(dev); + + opticalflowType_e opticalflowHardware = OPTICALFLOW_NONE; + requestedSensors[SENSOR_INDEX_OPTICALFLOW] = opticalflowHardwareToUse; + + switch (opticalflowHardwareToUse) { + case OPTICALFLOW_MT: +#ifdef USE_RANGEFINDER_MT + if (mtOpticalflowDetect(dev, rangefinderConfig()->rangefinder_hardware)) { + opticalflowHardware = OPTICALFLOW_MT; + rescheduleTask(TASK_OPTICALFLOW, TASK_PERIOD_MS(dev->delayMs)); + } +#endif + break; + + case OPTICALFLOW_NONE: + opticalflowHardware = OPTICALFLOW_NONE; + break; + } + + if (opticalflowHardware == OPTICALFLOW_NONE) { + sensorsClear(SENSOR_OPTICALFLOW); + return false; + } + + detectedSensors[SENSOR_INDEX_OPTICALFLOW] = opticalflowHardware; + sensorsSet(SENSOR_OPTICALFLOW); + return true; +} + +bool opticalflowInit(void) { + if (!opticalflowDetect(&opticalflow.dev, opticalflowConfig()->opticalflow_hardware)) { + return false; + } + + opticalflow.dev.init(&opticalflow.dev); + opticalflow.quality = OPTICALFLOW_NO_NEW_DATA; + opticalflow.rawFlowRates.x = 0; + opticalflow.rawFlowRates.y = 0; + opticalflow.processedFlowRates.x = 0; + opticalflow.processedFlowRates.y = 0; + opticalflow.timeStampUs = micros(); + + cosRotAngle = cosf(DEGREES_TO_RADIANS(opticalflowConfig()->rotation)); + sinRotAngle = sinf(DEGREES_TO_RADIANS(opticalflowConfig()->rotation)); + //low pass filter + if (opticalflowConfig()->flow_lpf != 0) { + const float flowCutoffHz = (float)opticalflowConfig()->flow_lpf / 100.0f; + const float flowGain = pt2FilterGain(flowCutoffHz, opticalflow.dev.delayMs / 1000.0f); + + pt2FilterInit(&xFlowLpf, flowGain); + pt2FilterInit(&yFlowLpf, flowGain); + } + return true; +} + +void opticalflowUpdate(void) { + if (opticalflow.dev.update) { + opticalflow.dev.update(&opticalflow.dev); + } +} + +void opticalflowProcess(void) { + opticalflowData_t data = {0}; + uint32_t deltaTimeUs = 0; + opticalflow.dev.read(&opticalflow.dev, &data); + + opticalflow.quality = data.quality; + deltaTimeUs = cmp32(data.timeStampUs, opticalflow.timeStampUs); + + if (deltaTimeUs != 0) { // New data + vector2_t raw = data.flowRate; + vector2_t processed; + + applySensorRotation(&processed, &raw); + applyLPF(&processed); + + opticalflow.rawFlowRates = raw; + opticalflow.processedFlowRates = processed; + opticalflow.timeStampUs = data.timeStampUs; + + // DEBUG SECTION + DEBUG_SET(DEBUG_OPTICALFLOW, 0, opticalflow.quality); + DEBUG_SET(DEBUG_OPTICALFLOW, 1, lrintf(opticalflow.rawFlowRates.x * 1000)); + DEBUG_SET(DEBUG_OPTICALFLOW, 2, lrintf(opticalflow.rawFlowRates.y * 1000)); + DEBUG_SET(DEBUG_OPTICALFLOW, 3, lrintf(opticalflow.processedFlowRates.x * 1000)); + DEBUG_SET(DEBUG_OPTICALFLOW, 4, lrintf(opticalflow.processedFlowRates.y * 1000)); + DEBUG_SET(DEBUG_OPTICALFLOW, 5, deltaTimeUs); + } +} + +static void applySensorRotation(vector2_t * dst, vector2_t * src) { + dst->x = (opticalflowConfig()->flip_x ? -1.0f : 1.0f) * (src->x * cosRotAngle - src->y * sinRotAngle); + dst->y = src->x * sinRotAngle + src->y * cosRotAngle; +} + +static void applyLPF(vector2_t * flowRates) { + if (opticalflowConfig()->flow_lpf == 0) { + return; + } + + flowRates->x = pt2FilterApply(&xFlowLpf, flowRates->x); + flowRates->y = pt2FilterApply(&yFlowLpf, flowRates->y); +} + +const opticalflow_t * getLatestFlowOpticalflowData(void) { + return &opticalflow; +} + +bool isOpticalflowHealthy(void) { + return cmp32(micros(), opticalflow.timeStampUs) < OPTICALFLOW_HARDWARE_TIMEOUT_US; +} +#endif // USE_OPTICALFLOW diff --git a/src/main/sensors/opticalflow.h b/src/main/sensors/opticalflow.h new file mode 100644 index 0000000000..e715aeb3f0 --- /dev/null +++ b/src/main/sensors/opticalflow.h @@ -0,0 +1,56 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software 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. + * + * Betaflight 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 software. + * + * If not, see . + */ +#pragma once + +#include +#include + +#include "drivers/opticalflow/opticalflow.h" + +#include "pg/pg.h" + +typedef enum { + OPTICALFLOW_NONE = 0, + OPTICALFLOW_MT = 1, +} opticalflowType_e; + +typedef struct opticalflowConfig_s { + uint8_t opticalflow_hardware; + uint16_t rotation; + uint8_t flip_x; + uint16_t flow_lpf; +} opticalflowConfig_t; + +PG_DECLARE(opticalflowConfig_t, opticalflowConfig); + +typedef struct opticalflow_s { + opticalflowDev_t dev; + int16_t quality; + vector2_t rawFlowRates; + vector2_t processedFlowRates; + uint32_t timeStampUs; +} opticalflow_t; + +bool opticalflowInit(void); + +void opticalflowUpdate(void); +bool isOpticalflowHealthy(void); +void opticalflowProcess(void); diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 80367d12f8..03c4b4a95c 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -26,6 +26,7 @@ typedef enum { SENSOR_INDEX_BARO, SENSOR_INDEX_MAG, SENSOR_INDEX_RANGEFINDER, + SENSOR_INDEX_OPTICALFLOW, SENSOR_INDEX_COUNT } sensorIndex_e; @@ -52,5 +53,6 @@ typedef enum { SENSOR_SONAR = 1 << 4, SENSOR_RANGEFINDER = 1 << 4, SENSOR_GPS = 1 << 5, - SENSOR_GPSMAG = 1 << 6 + SENSOR_GPSMAG = 1 << 6, + SENSOR_OPTICALFLOW = 1 << 7 } sensors_e;