src/main/sensors/opflow.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27 #include <string.h>
  28 #include <math.h>
  29
  30 #include <platform.h>
  31
  32 #include "build/build_config.h"
  33
  34 #include "common/axis.h"
  35 #include "common/maths.h"
  36 #include "common/utils.h"
  37 #include "common/time.h"
  38
  39 #include "config/feature.h"
  40 #include "config/parameter_group.h"
  41 #include "config/parameter_group_ids.h"
  42
  43 #include "drivers/io.h"
  44 #include "drivers/light_led.h"
  45 #include "drivers/time.h"
  46
  47 #include "drivers/opflow/opflow.h"
  48 #include "drivers/opflow/opflow_fake.h"
  49 #include "drivers/opflow/opflow_virtual.h"
  50
  51 #include "fc/config.h"
  52 #include "fc/runtime_config.h"
  53 #include "fc/settings.h"
  54
  55 #include "sensors/boardalignment.h"
  56 #include "sensors/gyro.h"
  57 #include "sensors/sensors.h"
  58 #include "sensors/opflow.h"
  59
  60 #include "scheduler/scheduler.h"
  61
  62 #include "io/opflow.h"
  63
  64 #include "build/debug.h"
  65
  66 opflow_t opflow;
  67
  68 #ifdef USE_OPFLOW
  69 static bool opflowIsCalibrating = false;
  70 static timeMs_t opflowCalibrationStartedAt;
  71 static float opflowCalibrationBodyAcc;
  72 static float opflowCalibrationFlowAcc;
  73
  74 #define OPFLOW_SQUAL_THRESHOLD_HIGH     35      // TBD
  75 #define OPFLOW_SQUAL_THRESHOLD_LOW      10      // TBD
  76 #define OPFLOW_UPDATE_TIMEOUT_US        200000  // At least 5Hz updates required
  77 #define OPFLOW_CALIBRATE_TIME_MS        30000   // 30 second calibration time
  78
  79 PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 2);
  80
  81 PG_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig,
  82     .opflow_hardware = SETTING_OPFLOW_HARDWARE_DEFAULT,
  83     .opflow_align = SETTING_ALIGN_OPFLOW_DEFAULT,
  84     .opflow_scale = SETTING_OPFLOW_SCALE_DEFAULT,
  85 );
  86
  87 static bool opflowDetect(opflowDev_t * dev, uint8_t opflowHardwareToUse)
  88 {
  89     opticalFlowSensor_e opflowHardware = OPFLOW_NONE;
  90     requestedSensors[SENSOR_INDEX_OPFLOW] = opflowHardwareToUse;
  91
  92     switch (opflowHardwareToUse) {
  93         case OPFLOW_FAKE:
  94 #if defined(USE_OPFLOW_FAKE)
  95             if (fakeOpflowDetect(dev)) {
  96                 opflowHardware = OPFLOW_FAKE;
  97             }
  98 #endif
  99             break;
 100
 101         case OPFLOW_CXOF:
 102 #if defined(USE_OPFLOW_CXOF)
 103             if (virtualOpflowDetect(dev, &opflowCxofVtable)) {
 104                 opflowHardware = OPFLOW_CXOF;
 105             }
 106 #endif
 107             break;
 108
 109         case OPFLOW_MSP:
 110 #if defined(USE_OPFLOW_MSP)
 111             if (virtualOpflowDetect(dev, &opflowMSPVtable)) {
 112                 opflowHardware = OPFLOW_MSP;
 113             }
 114 #endif
 115             break;
 116
 117         case OPFLOW_NONE:
 118             opflowHardware = OPFLOW_NONE;
 119             break;
 120     }
 121
 122     if (opflowHardware == OPFLOW_NONE) {
 123         sensorsClear(SENSOR_OPFLOW);
 124         return false;
 125     }
 126
 127     detectedSensors[SENSOR_INDEX_OPFLOW] = opflowHardware;
 128     sensorsSet(SENSOR_OPFLOW);
 129     return true;
 130 }
 131
 132 static void opflowZeroBodyGyroAcc(void)
 133 {
 134     opflow.gyroBodyRateTimeUs = 0;
 135     opflow.gyroBodyRateAcc[X] = 0;
 136     opflow.gyroBodyRateAcc[Y] = 0;
 137 }
 138
 139 bool opflowInit(void)
 140 {
 141     if (!opflowDetect(&opflow.dev, opticalFlowConfig()->opflow_hardware)) {
 142         return false;
 143     }
 144
 145     if (!opflow.dev.initFn(&opflow.dev)) {
 146         sensorsClear(SENSOR_OPFLOW);
 147         return false;
 148     }
 149
 150     opflowZeroBodyGyroAcc();
 151
 152     return true;
 153 }
 154
 155 void opflowStartCalibration(void)
 156 {
 157     opflowCalibrationStartedAt = millis();
 158     opflowIsCalibrating = true;
 159     opflowCalibrationBodyAcc = 0;
 160     opflowCalibrationFlowAcc = 0;
 161 }
 162
 163 /*
 164  * This is called periodically by the scheduler
 165  */
 166 void opflowUpdate(timeUs_t currentTimeUs)
 167 {
 168     if (!opflow.dev.updateFn)
 169         return;
 170
 171     if (opflow.dev.updateFn(&opflow.dev)) {
 172         // Indicate valid update
 173         opflow.isHwHealty = true;
 174         opflow.lastValidUpdate = currentTimeUs;
 175         opflow.rawQuality = opflow.dev.rawData.quality;
 176
 177         // Handle state switching
 178         switch (opflow.flowQuality) {
 179             case OPFLOW_QUALITY_INVALID:
 180                 if (opflow.dev.rawData.quality >= OPFLOW_SQUAL_THRESHOLD_HIGH) {
 181                     opflow.flowQuality = OPFLOW_QUALITY_VALID;
 182                 }
 183                 break;
 184
 185             case OPFLOW_QUALITY_VALID:
 186                 if (opflow.dev.rawData.quality <= OPFLOW_SQUAL_THRESHOLD_LOW) {
 187                     opflow.flowQuality = OPFLOW_QUALITY_INVALID;
 188                 }
 189                 break;
 190         }
 191
 192         // Opflow updated. Assume zero valus unless further processing sets otherwise
 193         opflow.flowRate[X] = 0;
 194         opflow.flowRate[Y] = 0;
 195         opflow.bodyRate[X] = 0;
 196         opflow.bodyRate[Y] = 0;
 197
 198         // In the following code we operate deg/s and do conversion to rad/s in the last step
 199         // Calculate body rates
 200         if (opflow.gyroBodyRateTimeUs > 0) {
 201             opflow.bodyRate[X] = opflow.gyroBodyRateAcc[X] / opflow.gyroBodyRateTimeUs;
 202             opflow.bodyRate[Y] = opflow.gyroBodyRateAcc[Y] / opflow.gyroBodyRateTimeUs;
 203         }
 204
 205         // If quality of the flow from the sensor is good - process further
 206         if (opflow.flowQuality == OPFLOW_QUALITY_VALID) {
 207             const float integralToRateScaler = (opticalFlowConfig()->opflow_scale > 0.01f) ? (1.0e6 / opflow.dev.rawData.deltaTime) / (float)opticalFlowConfig()->opflow_scale : 0.0f;
 208
 209             // Apply sensor alignment
 210             applySensorAlignment(opflow.dev.rawData.flowRateRaw, opflow.dev.rawData.flowRateRaw, opticalFlowConfig()->opflow_align);
 211
 212             // Calculate flow rate and accumulated body rate
 213             opflow.flowRate[X] = opflow.dev.rawData.flowRateRaw[X] * integralToRateScaler;
 214             opflow.flowRate[Y] = opflow.dev.rawData.flowRateRaw[Y] * integralToRateScaler;
 215
 216             // Only update DEBUG_FLOW_RAW if flow is good
 217             DEBUG_SET(DEBUG_FLOW_RAW, 0, (opflow.flowRate[X]));
 218             DEBUG_SET(DEBUG_FLOW_RAW, 1, (opflow.flowRate[Y]));
 219             DEBUG_SET(DEBUG_FLOW_RAW, 2, (opflow.bodyRate[X]));
 220             DEBUG_SET(DEBUG_FLOW_RAW, 3, (opflow.bodyRate[Y]));
 221         }
 222
 223         // Process calibration
 224         if (opflowIsCalibrating) {
 225             // Blink LED
 226             LED0_TOGGLE;
 227
 228             if ((millis() - opflowCalibrationStartedAt) > OPFLOW_CALIBRATE_TIME_MS) {
 229                 // Finish calibration if we accumulated more than 3600 deg of rotation over 30 seconds
 230                 if (opflowCalibrationBodyAcc > 3600.0f) {
 231                     opticalFlowConfigMutable()->opflow_scale = opflowCalibrationFlowAcc / opflowCalibrationBodyAcc;
 232                     saveConfigAndNotify();
 233                 }
 234
 235                 opflowIsCalibrating = 0;
 236             }
 237             else if (opflow.flowQuality == OPFLOW_QUALITY_VALID) {
 238                 // Ongoing calibration - accumulate body and flow rotation magniture if opflow quality is good enough
 239                 const float invDt = 1.0e6 / opflow.dev.rawData.deltaTime;
 240                 opflowCalibrationBodyAcc += calc_length_pythagorean_2D(opflow.bodyRate[X], opflow.bodyRate[Y]);
 241                 opflowCalibrationFlowAcc += calc_length_pythagorean_2D(opflow.dev.rawData.flowRateRaw[X], opflow.dev.rawData.flowRateRaw[Y]) * invDt;
 242             }
 243         }
 244
 245         // Convert to radians so NAV doesn't have to do the conversion
 246         opflow.bodyRate[X] = DEGREES_TO_RADIANS(opflow.bodyRate[X]);
 247         opflow.bodyRate[Y] = DEGREES_TO_RADIANS(opflow.bodyRate[Y]);
 248         opflow.flowRate[X] = DEGREES_TO_RADIANS(opflow.flowRate[X]);
 249         opflow.flowRate[Y] = DEGREES_TO_RADIANS(opflow.flowRate[Y]);
 250
 251         // Zero out gyro accumulators to calculate rotation per flow update
 252         opflowZeroBodyGyroAcc();
 253     }
 254     else {
 255         // No new data available
 256         if (opflow.isHwHealty && ((currentTimeUs - opflow.lastValidUpdate) > OPFLOW_UPDATE_TIMEOUT_US)) {
 257             opflow.isHwHealty = false;
 258
 259             opflow.flowQuality = OPFLOW_QUALITY_INVALID;
 260             opflow.rawQuality = 0;
 261
 262             opflow.flowRate[X] = 0;
 263             opflow.flowRate[Y] = 0;
 264             opflow.bodyRate[X] = 0;
 265             opflow.bodyRate[Y] = 0;
 266
 267             opflowZeroBodyGyroAcc();
 268         }
 269     }
 270 }
 271
 272 /* Run a simple gyro update integrator to estimate average body rate between two optical flow updates */
 273 void opflowGyroUpdateCallback(timeUs_t gyroUpdateDeltaUs)
 274 {
 275     if (!opflow.isHwHealty)
 276         return;
 277
 278     for (int axis = 0; axis < 2; axis++) {
 279         opflow.gyroBodyRateAcc[axis] += gyro.gyroADCf[axis] * gyroUpdateDeltaUs;
 280     }
 281
 282     opflow.gyroBodyRateTimeUs += gyroUpdateDeltaUs;
 283 }
 284
 285 bool opflowIsHealthy(void)
 286 {
 287     return opflow.isHwHealty;
 288 }
 289 #endif