Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-12 19:10:32 +03:00
Code Issues Wiki Activity

Merge ada295f0eb into acbab53d13

Browse source
This commit is contained in:
Ahmed Mansour 2025-07-11 16:43:59 +02:00 • committed by GitHub
commit 7f5b021616
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
4 changed files with 18 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/cli/settings.c
View file

@ -1800,6 +1800,7 @@ const clivalue_t valueTable[] = {
// PG_RANGEFINDER_CONFIG
#ifdef USE_RANGEFINDER
{ "rangefinder_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RANGEFINDER_HARDWARE }, PG_RANGEFINDER_CONFIG, offsetof(rangefinderConfig_t, rangefinder_hardware) },
{ "rangefinder_trust" , VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 } , PG_RANGEFINDER_CONFIG, offsetof(rangefinderConfig_t, rangefinder_trust) },
#endif
// PG_OPTICALFLOW_CONFIG

14
src/main/flight/position.c
View file

@ -44,6 +44,10 @@
#include "sensors/sensors.h"
#include "sensors/barometer.h"
#ifdef USE_RANGEFINDER
#include "sensors/rangefinder.h"
#endif
#include "pg/pg.h"
#include "pg/pg_ids.h"
@ -127,6 +131,10 @@ void calculateEstimatedAltitude(void)
}
#endif
#ifdef USE_RANGEFINDER
float rangefinderAltCm = rangefinderGetLatestAltitude();;
float rangefinderTrust = (float)(rangefinderConfig()->rangefinder_trust) / 100.0f;
#endif
// *** DISARMED ***
if (!ARMING_FLAG(ARMED)) {
if (wasArmed) { // things to run once, on disarming, after being armed
@ -183,6 +191,12 @@ void calculateEstimatedAltitude(void)
zeroedAltitudeCm = baroAltCm; // use Baro if no GPS data, or we want Baro only
}
}
#ifdef USE_RANGEFINDER
if (rangefinderAltCm > 0) {
// mix it with the range finder
zeroedAltitudeCm = (rangefinderTrust * rangefinderAltCm) + ((1.0 - rangefinderTrust) * zeroedAltitudeCm);
}
#endif
zeroedAltitudeCm = pt2FilterApply(&altitudeLpf, zeroedAltitudeCm);
// NOTE: this filter must receive 0 as its input, for the whole disarmed time, to ensure correct zeroed values on arming

3
src/main/sensors/rangefinder.c
View file

@ -70,10 +70,11 @@ rangefinder_t rangefinder;
#define RANGEFINDER_DYNAMIC_THRESHOLD 600 //Used to determine max. usable rangefinder disatance
#define RANGEFINDER_DYNAMIC_FACTOR 75
PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 1);
PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig,
.rangefinder_hardware = RANGEFINDER_NONE,
.rangefinder_trust = 0,
);
#ifdef USE_RANGEFINDER_HCSR04

1
src/main/sensors/rangefinder.h
View file

@ -40,6 +40,7 @@ typedef enum {
typedef struct rangefinderConfig_s {
uint8_t rangefinder_hardware;
uint8_t rangefinder_trust;
} rangefinderConfig_t;
PG_DECLARE(rangefinderConfig_t, rangefinderConfig);