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

Gps Rescue: Fix "velocityD" filtering (#12042)

Browse source 
* GPS Rescue: Refactor parameter group

* GPS Rescue: Fix "velocityD" filtering
This commit is contained in:
Jan Post 2022-12-10 13:41:35 +01:00 • committed by GitHub
parent 7bd81bf639
commit cb41b55c40
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
10 changed files with 210 additions and 111 deletions
Download patch file Download diff file Expand all files Collapse all files

96
src/main/flight/gps_rescue.c
View file

@ -26,6 +26,7 @@
#include "build/debug.h"
#include "common/axis.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
@ -44,21 +45,12 @@
#include "flight/pid.h"
#include "flight/position.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "rx/rx.h"
#include "sensors/acceleration.h"
#include "gps_rescue.h"
typedef enum {
RESCUE_SANITY_OFF = 0,
RESCUE_SANITY_ON,
RESCUE_SANITY_FS_ONLY
} gpsRescueSanity_e;
typedef enum {
RESCUE_IDLE,
RESCUE_INITIALIZE,
@ -113,7 +105,6 @@ typedef struct {
float velocityToHomeCmS;
float alitutudeStepCm;
float maxPitchStep;
float filterK;
float absErrorAngle;
} rescueSensorData_s;
@ -125,12 +116,6 @@ typedef struct {
bool isAvailable;
} rescueState_s;
typedef enum {
MAX_ALT,
FIXED_ALT,
CURRENT_ALT
} altitudeMode_e;
#define GPS_RESCUE_MAX_YAW_RATE 180 // deg/sec max yaw rate
#define GPS_RESCUE_MIN_DESCENT_DIST_M 5 // minimum descent distance
#define GPS_RESCUE_MAX_ITERM_VELOCITY 1000 // max iterm value for velocity
@ -138,69 +123,33 @@ typedef enum {
#define GPS_RESCUE_MAX_PITCH_RATE 3000 // max change in pitch per second in degrees * 100
#define GPS_RESCUE_DISARM_THRESHOLD 2.0f // disarm threshold in G's
#ifdef USE_MAG
#define GPS_RESCUE_USE_MAG true
#else
#define GPS_RESCUE_USE_MAG false
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 3);
PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.minRescueDth = 30,
.altitudeMode = MAX_ALT,
.rescueAltitudeBufferM = 10,
.ascendRate = 500, // cm/s, for altitude corrections on ascent
.initialAltitudeM = 30,
.rescueGroundspeed = 500,
.angle = 40,
.rollMix = 150,
.descentDistanceM = 20,
.descendRate = 100, // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
.targetLandingAltitudeM = 4,
.throttleMin = 1100,
.throttleMax = 1600,
.throttleHover = 1275,
.allowArmingWithoutFix = false,
.sanityChecks = RESCUE_SANITY_FS_ONLY,
.minSats = 8,
.throttleP = 15,
.throttleI = 15,
.throttleD = 15,
.velP = 8,
.velI = 30,
.velD = 20,
.yawP = 20,
.useMag = GPS_RESCUE_USE_MAG
);
static float rescueThrottle;
static float rescueYaw;
float gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
bool magForceDisable = false;
static bool newGPSData = false;
static pt2Filter_t throttleDLpf;
static pt2Filter_t velocityDLpf;
static pt3Filter_t pitchLpf;
rescueState_s rescueState;
void gpsRescueInit(void)
{
const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
const float sampleTimeS = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ);
float cutoffHz, gain;
const float throttleDCutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
const float throttleDCutoffGain = pt2FilterGain(throttleDCutoffHz, sampleTimeS);
pt2FilterInit(&throttleDLpf, throttleDCutoffGain);
cutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
gain = pt2FilterGain(cutoffHz, sampleTimeS);
pt2FilterInit(&throttleDLpf, gain);
const float pitchCutoffHz = 4.0f;
const float pitchCutoffGain = pt3FilterGain(pitchCutoffHz, sampleTimeS);
pt3FilterInit(&pitchLpf, pitchCutoffGain);
cutoffHz = 0.8f;
gain = pt2FilterGain(cutoffHz, 1.0f);
pt2FilterInit(&velocityDLpf, gain);
cutoffHz = 4.0f;
gain = pt3FilterGain(cutoffHz, sampleTimeS);
pt3FilterInit(&pitchLpf, gain);
}
/*
@ -242,13 +191,13 @@ static void setReturnAltitude(void)
const float initialAltitudeCm = gpsRescueConfig()->initialAltitudeM * 100.0f;
const float rescueAltitudeBufferCm = gpsRescueConfig()->rescueAltitudeBufferM * 100.0f;
switch (gpsRescueConfig()->altitudeMode) {
case FIXED_ALT:
case GPS_RESCUE_ALT_MODE_FIXED:
rescueState.intent.returnAltitudeCm = initialAltitudeCm;
break;
case CURRENT_ALT:
case GPS_RESCUE_ALT_MODE_CURRENT:
rescueState.intent.returnAltitudeCm = rescueState.sensor.currentAltitudeCm + rescueAltitudeBufferCm;
break;
case MAX_ALT:
case GPS_RESCUE_ALT_MODE_MAX:
default:
rescueState.intent.returnAltitudeCm = rescueState.intent.maxAltitudeCm + rescueAltitudeBufferCm;
break;
@ -261,7 +210,6 @@ static void rescueAttainPosition(void)
// runs at 100hz, but only updates RPYT settings when new GPS Data arrives and when not in idle phase.
static float previousVelocityError = 0.0f;
static float velocityI = 0.0f;
static float previousVelocityD = 0.0f; // for smoothing
static float previousPitchAdjustment = 0.0f;
static float throttleI = 0.0f;
static float previousAltitudeError = 0.0f;
@ -278,7 +226,6 @@ static void rescueAttainPosition(void)
// Initialize internal variables each time GPS Rescue is started
previousVelocityError = 0.0f;
velocityI = 0.0f;
previousVelocityD = 0.0f;
previousPitchAdjustment = 0.0f;
throttleI = 0.0f;
previousAltitudeError = 0.0f;
@ -391,9 +338,9 @@ static void rescueAttainPosition(void)
// D component
float velocityD = ((velocityError - previousVelocityError) / sampleIntervalNormaliseFactor);
previousVelocityError = velocityError;
// simple first order filter on derivative with k = 0.5 for 200ms steps
velocityD = previousVelocityD + rescueState.sensor.filterK * (velocityD - previousVelocityD);
previousVelocityD = velocityD;
const float gain = pt2FilterGain(0.8f, HZ_TO_INTERVAL(gpsGetSampleRateHz()));
pt2FilterUpdateCutoff(&velocityDLpf, gain);
velocityD = pt2FilterApply(&velocityDLpf, velocityD);
velocityD *= gpsRescueConfig()->velD;
const float velocityIAttenuator = rescueState.intent.targetVelocityCmS / gpsRescueConfig()->rescueGroundspeed;
@ -602,9 +549,6 @@ static void sensorUpdate(void)
// Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values.
previousGPSDataTimeUs = currentTimeUs;
rescueState.sensor.filterK = pt1FilterGain(0.8, rescueState.sensor.gpsDataIntervalSeconds);
// 0.8341 for 1hz, 0.5013 for 5hz, 0.3345 for 10hz, 0.1674 for 25Hz, etc
rescueState.sensor.velocityToHomeCmS = (prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) / rescueState.sensor.gpsDataIntervalSeconds;
// positive = towards home. First value is useless since prevDistanceToHomeCm was zero.
prevDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;

48
src/main/flight/gps_rescue.h
View file

@ -17,37 +17,33 @@
#pragma once
#include <stdbool.h>
#include "common/axis.h"
#include "pg/pg.h"
#include "pg/gps_rescue.h"
#define TASK_GPS_RESCUE_RATE_HZ 100 // synced to altitude task rate
#define TASK_GPS_RESCUE_RATE_HZ 100 // in sync with altitude task rate
typedef struct gpsRescue_s {
uint16_t angle; // degrees
uint16_t initialAltitudeM; // meters
uint16_t descentDistanceM; // meters
uint16_t rescueGroundspeed; // centimeters per second
uint8_t throttleP, throttleI, throttleD;
uint8_t yawP;
uint16_t throttleMin;
uint16_t throttleMax;
uint16_t throttleHover;
uint8_t minSats;
uint8_t velP, velI, velD;
uint16_t minRescueDth; // meters
uint8_t sanityChecks;
uint8_t allowArmingWithoutFix;
uint8_t useMag;
uint8_t targetLandingAltitudeM; // meters
uint8_t altitudeMode;
uint16_t ascendRate;
uint16_t descendRate;
uint16_t rescueAltitudeBufferM; // meters
uint8_t rollMix;
} gpsRescueConfig_t;
#ifdef USE_MAG
#define GPS_RESCUE_USE_MAG true
#else
#define GPS_RESCUE_USE_MAG false
#endif
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
typedef enum {
RESCUE_SANITY_OFF = 0,
RESCUE_SANITY_ON,
RESCUE_SANITY_FS_ONLY,
RESCUE_SANITY_COUNT
} gpsRescueSanity_e;
typedef enum {
GPS_RESCUE_ALT_MODE_MAX = 0,
GPS_RESCUE_ALT_MODE_FIXED,
GPS_RESCUE_ALT_MODE_CURRENT,
GPS_RESCUE_ALT_MODE_COUNT
} gpsRescueAltitudeMode_e;
extern float gpsRescueAngle[ANGLE_INDEX_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES

18
src/main/io/gps.c
View file

@ -107,6 +107,7 @@ uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N];
#define UBLOX_ACK_TIMEOUT_MAX_COUNT (25)
static serialPort_t *gpsPort;
static float gpsSampleRateHz;
typedef struct gpsInitData_s {
uint8_t index;
@ -307,6 +308,8 @@ static void gpsSetState(gpsState_e state)
void gpsInit(void)
{
gpsSampleRateHz = 0.0f;
gpsData.baudrateIndex = 0;
gpsData.errors = 0;
gpsData.timeouts = 0;
@ -1904,6 +1907,13 @@ void GPS_calculateDistanceAndDirectionToHome(void)
void onGpsNewData(void)
{
static timeUs_t timeUs, lastTimeUs = 0;
// Detect current sample rate of GPS solution
timeUs = micros();
gpsSampleRateHz = 1e6f / cmpTimeUs(timeUs, lastTimeUs);
lastTimeUs = timeUs;
if (!(STATE(GPS_FIX) && gpsSol.numSat >= GPS_MIN_SAT_COUNT)) {
// if we don't have a 3D fix and the minimum sats, don't give data to GPS rescue
return;
@ -1928,4 +1938,10 @@ void gpsSetFixState(bool state)
DISABLE_STATE(GPS_FIX);
}
}
#endif
float gpsGetSampleRateHz(void)
{
return gpsSampleRateHz;
}
#endif // USE_GPS

1
src/main/io/gps.h
View file

@ -214,3 +214,4 @@ void GPS_reset_home_position(void);
void GPS_calc_longitude_scaling(int32_t lat);
void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing);
void gpsSetFixState(bool state);
float gpsGetSampleRateHz(void);

69
src/main/pg/gps_rescue.c Normal file
View file

@ -0,0 +1,69 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are 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.
*
* Cleanflight and Betaflight are distributed in the hope that they
* 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 "platform.h"
#ifdef USE_GPS_RESCUE
#include "flight/gps_rescue.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "gps_rescue.h"
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 3);
PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.minRescueDth = 30,
.altitudeMode = GPS_RESCUE_ALT_MODE_MAX,
.rescueAltitudeBufferM = 10,
.ascendRate = 500, // cm/s, for altitude corrections on ascent
.initialAltitudeM = 30,
.rescueGroundspeed = 500,
.angle = 40,
.rollMix = 150,
.descentDistanceM = 20,
.descendRate = 100, // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
.targetLandingAltitudeM = 4,
.throttleMin = 1100,
.throttleMax = 1600,
.throttleHover = 1275,
.allowArmingWithoutFix = false,
.sanityChecks = RESCUE_SANITY_FS_ONLY,
.minSats = 8,
.throttleP = 15,
.throttleI = 15,
.throttleD = 15,
.velP = 8,
.velI = 30,
.velD = 20,
.yawP = 20,
.useMag = GPS_RESCUE_USE_MAG
);
#endif // USE_GPS_RESCUE

53
src/main/pg/gps_rescue.h Normal file
View file

@ -0,0 +1,53 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are 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.
*
* Cleanflight and Betaflight are distributed in the hope that they
* 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/>.
*/
#pragma once
#include <stdint.h>
#include "pg/pg.h"
typedef struct gpsRescue_s {
uint16_t angle; // degrees
uint16_t initialAltitudeM; // meters
uint16_t descentDistanceM; // meters
uint16_t rescueGroundspeed; // centimeters per second
uint8_t throttleP, throttleI, throttleD;
uint8_t yawP;
uint16_t throttleMin;
uint16_t throttleMax;
uint16_t throttleHover;
uint8_t minSats;
uint8_t velP, velI, velD;
uint16_t minRescueDth; // meters
uint8_t sanityChecks;
uint8_t allowArmingWithoutFix;
uint8_t useMag;
uint8_t targetLandingAltitudeM; // meters
uint8_t altitudeMode;
uint16_t ascendRate;
uint16_t descendRate;
uint16_t rescueAltitudeBufferM; // meters
uint8_t rollMix;
} gpsRescueConfig_t;
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);

4
src/test/Makefile
View file

@ -35,13 +35,13 @@ alignsensor_unittest_SRC := \
$(USER_DIR)/common/maths.c
arming_prevention_unittest_SRC := \
$(USER_DIR)/common/bitarray.c \
$(USER_DIR)/fc/core.c \
$(USER_DIR)/fc/dispatch.c \
$(USER_DIR)/fc/rc_controls.c \
$(USER_DIR)/fc/rc_modes.c \
$(USER_DIR)/fc/runtime_config.c \
$(USER_DIR)/flight/gps_rescue.c \
$(USER_DIR)/common/bitarray.c
$(USER_DIR)/flight/gps_rescue.c
arming_prevention_unittest_DEFINES := \
USE_GPS_RESCUE=

30
src/test/unit/arming_prevention_unittest.cc
View file

@ -18,34 +18,48 @@
#include <stdint.h>
extern "C" {
#include "blackbox/blackbox.h"
#include "build/debug.h"
#include "common/filter.h"
#include "common/maths.h"
#include "config/feature.h"
#include "pg/motor.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/rx.h"
#include "config/config.h"
#include "config/feature.h"
#include "fc/controlrate_profile.h"
#include "fc/core.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/failsafe.h"
#include "flight/gps_rescue.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/position.h"
#include "flight/servos.h"
#include "io/beeper.h"
#include "io/gps.h"
#include "pg/gps_rescue.h"
#include "pg/motor.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/rx.h"
#include "rx/rx.h"
#include "scheduler/scheduler.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "telemetry/telemetry.h"
#include "flight/gps_rescue.h"
PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
@ -60,6 +74,7 @@ extern "C" {
PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0);
float rcCommand[4];
@ -81,6 +96,9 @@ extern "C" {
acc_t acc = {};
bool mockIsUpright = false;
uint8_t activePidLoopDenom = 1;
float gpsGetSampleRateHz(void) { return 10.0f; }
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k) { filter->k = k; }
}
uint32_t simulationFeatureFlags = 0;

1
src/test/unit/osd_unittest.cc
View file

@ -54,6 +54,7 @@ extern "C" {
#include "osd/osd_elements.h"
#include "osd/osd_warnings.h"
#include "pg/gps_rescue.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/rx.h"

1
src/test/unit/vtx_unittest.cc
View file

@ -43,6 +43,7 @@ extern "C" {
#include "io/gps.h"
#include "io/vtx.h"
#include "pg/gps_rescue.h"
#include "pg/motor.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"