mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-25 17:25:20 +03:00
opticalflow mt sensor added (#14038)
* opticalflow mt sensor added * set the processedFlow to raw when no rotation * use sin and cos approximations Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/sensors/opticalflow.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * refactoring * not needed anymore * update * Update src/main/sensors/opticalflow.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/sensors/opticalflow.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/drivers/rangefinder/rangefinder_lidarmt.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * update * rm prototype * clean up * Update src/main/drivers/opticalflow/opticalflow.h Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/drivers/opticalflow/opticalflow.h Co-authored-by: Mark Haslinghuis <mark@numloq.nl> * update flip_y * Update src/main/drivers/opticalflow/opticalflow.h Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/drivers/rangefinder/rangefinder_lidarmt.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/drivers/rangefinder/rangefinder_lidarmt.h Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/sensors/opticalflow.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/sensors/opticalflow.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/sensors/opticalflow.c Co-authored-by: Mark Haslinghuis <mark@numloq.nl> * rm casting * update op rotation --------- Co-authored-by: Petr Ledvina <ledvinap@gmail.com> Co-authored-by: Mark Haslinghuis <mark@numloq.nl>
This commit is contained in:
ahmedsalah52
GitHub
parent
2f21754a69
commit
d244239f39
21 changed files with 490 additions and 21 deletions
196
src/main/sensors/opticalflow.c
Normal file
196
src/main/sensors/opticalflow.c
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#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
|
||||
Loading…
Add table
Add a link
Reference in a new issue