src/main/sensors/opticalflow.c

   1 /*
   2  * This file is part of Betaflight.
   3  *
   4  * Betaflight is free software. You can redistribute this software
   5  * and/or modify this software under the terms of the GNU General
   6  * Public License as published by the Free Software Foundation,
   7  * either version 3 of the License, or (at your option) any later
   8  * version.
   9  *
  10  * Betaflight is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  *
  14  * See the GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public
  17  * License along with this software.
  18  *
  19  * If not, see <http://www.gnu.org/licenses/>.
  20  */
  21
  22
  23 #include <stdint.h>
  24 #include <math.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_OPTICALFLOW
  29
  30 #include "build/build_config.h"
  31 #include "build/debug.h"
  32
  33 #include "common/maths.h"
  34 #include "common/time.h"
  35 #include "common/utils.h"
  36 #include "common/filter.h"
  37
  38 #include "config/config.h"
  39 #include "config/feature.h"
  40
  41 #include "fc/runtime_config.h"
  42
  43 #include "scheduler/scheduler.h"
  44
  45 #include "pg/pg.h"
  46 #include "pg/pg_ids.h"
  47
  48 #include "drivers/time.h"
  49 #include "drivers/rangefinder/rangefinder.h"
  50 #include "drivers/rangefinder/rangefinder_lidarmt.h"
  51
  52 #include "io/beeper.h"
  53
  54 #include "sensors/sensors.h"
  55 #include "sensors/gyro.h"
  56 #include "sensors/acceleration.h"
  57 #include "sensors/opticalflow.h"
  58
  59 #define OPTICALFLOW_CALIBRATION_DURATION_MS 30000
  60 #define RATE_SCALE_RESOLUTION (1000.0f)
  61
  62 // static prototypes
  63 static void applySensorRotation(vector2_t * dst, vector2_t * src);
  64 static void applyLPF(vector2_t * flowRates);
  65
  66 PG_REGISTER_WITH_RESET_TEMPLATE(opticalflowConfig_t, opticalflowConfig, PG_OPTICALFLOW_CONFIG, 0);
  67
  68 PG_RESET_TEMPLATE(opticalflowConfig_t, opticalflowConfig,
  69     .opticalflow_hardware = OPTICALFLOW_NONE,
  70     .rotation = 0,
  71     .flow_lpf = 0,
  72     .flip_x = 0
  73 );
  74
  75 static opticalflow_t opticalflow;
  76 float cosRotAngle = 1.0f;
  77 float sinRotAngle = 0.0f;
  78 static pt2Filter_t xFlowLpf, yFlowLpf;
  79
  80 // ======================================================================
  81 // =================== Opticalflow Main Functions =======================
  82 // ======================================================================
  83 static bool opticalflowDetect(opticalflowDev_t * dev, uint8_t opticalflowHardwareToUse) {
  84     UNUSED(dev);
  85
  86     opticalflowType_e opticalflowHardware = OPTICALFLOW_NONE;
  87     requestedSensors[SENSOR_INDEX_OPTICALFLOW] = opticalflowHardwareToUse;
  88
  89     switch (opticalflowHardwareToUse) {
  90         case OPTICALFLOW_MT:
  91 #ifdef USE_RANGEFINDER_MT
  92             if (mtOpticalflowDetect(dev, rangefinderConfig()->rangefinder_hardware)) {
  93                 opticalflowHardware = OPTICALFLOW_MT;
  94                 rescheduleTask(TASK_OPTICALFLOW, TASK_PERIOD_MS(dev->delayMs));
  95             }
  96 #endif
  97             break;
  98
  99         case OPTICALFLOW_NONE:
 100             opticalflowHardware = OPTICALFLOW_NONE;
 101             break;
 102     }
 103
 104     if (opticalflowHardware == OPTICALFLOW_NONE) {
 105         sensorsClear(SENSOR_OPTICALFLOW);
 106         return false;
 107     }
 108
 109     detectedSensors[SENSOR_INDEX_OPTICALFLOW] = opticalflowHardware;
 110     sensorsSet(SENSOR_OPTICALFLOW);
 111     return true;
 112 }
 113
 114 bool opticalflowInit(void) {
 115     if (!opticalflowDetect(&opticalflow.dev, opticalflowConfig()->opticalflow_hardware)) {
 116         return false;
 117     }
 118
 119     opticalflow.dev.init(&opticalflow.dev);
 120     opticalflow.quality = OPTICALFLOW_NO_NEW_DATA;
 121     opticalflow.rawFlowRates.x = 0;
 122     opticalflow.rawFlowRates.y = 0;
 123     opticalflow.processedFlowRates.x = 0;
 124     opticalflow.processedFlowRates.y = 0;
 125     opticalflow.timeStampUs = micros();
 126
 127     cosRotAngle = cosf(DEGREES_TO_RADIANS(opticalflowConfig()->rotation));
 128     sinRotAngle = sinf(DEGREES_TO_RADIANS(opticalflowConfig()->rotation));
 129     //low pass filter
 130     if (opticalflowConfig()->flow_lpf != 0) {
 131         const float flowCutoffHz = (float)opticalflowConfig()->flow_lpf / 100.0f;
 132         const float flowGain     = pt2FilterGain(flowCutoffHz, opticalflow.dev.delayMs / 1000.0f);
 133
 134         pt2FilterInit(&xFlowLpf, flowGain);
 135         pt2FilterInit(&yFlowLpf, flowGain);
 136     }
 137     return true;
 138 }
 139
 140 void opticalflowUpdate(void) {
 141     if (opticalflow.dev.update) {
 142         opticalflow.dev.update(&opticalflow.dev);
 143     }
 144 }
 145
 146 void opticalflowProcess(void) {
 147     opticalflowData_t data = {0};
 148     uint32_t deltaTimeUs = 0;
 149     opticalflow.dev.read(&opticalflow.dev, &data);
 150
 151     opticalflow.quality = data.quality;
 152     deltaTimeUs = cmp32(data.timeStampUs, opticalflow.timeStampUs);
 153
 154     if (deltaTimeUs != 0) { // New data
 155         vector2_t raw = data.flowRate;
 156         vector2_t processed;
 157
 158         applySensorRotation(&processed, &raw);
 159         applyLPF(&processed);
 160
 161         opticalflow.rawFlowRates = raw;
 162         opticalflow.processedFlowRates = processed;
 163         opticalflow.timeStampUs  = data.timeStampUs;
 164
 165         // DEBUG SECTION
 166         DEBUG_SET(DEBUG_OPTICALFLOW, 0, opticalflow.quality);
 167         DEBUG_SET(DEBUG_OPTICALFLOW, 1, lrintf(opticalflow.rawFlowRates.x * 1000));
 168         DEBUG_SET(DEBUG_OPTICALFLOW, 2, lrintf(opticalflow.rawFlowRates.y * 1000));
 169         DEBUG_SET(DEBUG_OPTICALFLOW, 3, lrintf(opticalflow.processedFlowRates.x * 1000));
 170         DEBUG_SET(DEBUG_OPTICALFLOW, 4, lrintf(opticalflow.processedFlowRates.y * 1000));
 171         DEBUG_SET(DEBUG_OPTICALFLOW, 5, deltaTimeUs);
 172     }
 173 }
 174
 175 static void applySensorRotation(vector2_t * dst, vector2_t * src) {
 176     dst->x = (opticalflowConfig()->flip_x ? -1.0f : 1.0f) * (src->x * cosRotAngle - src->y * sinRotAngle);
 177     dst->y = src->x * sinRotAngle + src->y * cosRotAngle;
 178 }
 179
 180 static void applyLPF(vector2_t * flowRates) {
 181     if (opticalflowConfig()->flow_lpf == 0) {
 182         return;
 183     }
 184
 185     flowRates->x = pt2FilterApply(&xFlowLpf, flowRates->x);
 186     flowRates->y = pt2FilterApply(&yFlowLpf, flowRates->y);
 187 }
 188
 189 const opticalflow_t * getLatestFlowOpticalflowData(void) {
 190     return &opticalflow;
 191 }
 192
 193 bool isOpticalflowHealthy(void) {
 194     return cmp32(micros(), opticalflow.timeStampUs) < OPTICALFLOW_HARDWARE_TIMEOUT_US;
 195 }
 196 #endif // USE_OPTICALFLOW