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

Revise yaw rate calculation and defaults

Browse source
This commit is contained in:
Bruce Luckcuck 2018-06-27 11:10:52 -04:00
parent 9868557c11
commit 0cd61c3a0f
1 changed files with 7 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -50,8 +50,8 @@
#include "gps_rescue.h"
#define GPS_RESCUE_MAX_YAW_RATE 180 // 180 deg/sec
#define GPS_RESCUE_RATE_SCALE_DEGREES 20 // Scale the commanded yaw rate when the error is less then this angle
#define GPS_RESCUE_MAX_YAW_RATE 100 // 100 deg/sec max yaw
#define GPS_RESCUE_RATE_SCALE_DEGREES 45 // Scale the commanded yaw rate when the error is less then this angle
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
@ -421,6 +421,11 @@ void setBearing(int16_t desiredHeading)
// Calculate a desired yaw rate based on a maximum limit beyond
// an error window and then scale the requested rate down inside
// the window as error approaches 0.
if (errorAngleAbs > GPS_RESCUE_RATE_SCALE_DEGREES) {
rescueYaw = GPS_RESCUE_MAX_YAW_RATE;
} else {
rescueYaw = (errorAngleAbs / GPS_RESCUE_RATE_SCALE_DEGREES) * GPS_RESCUE_MAX_YAW_RATE;
}
rescueYaw = (errorAngleAbs > GPS_RESCUE_RATE_SCALE_DEGREES) ? GPS_RESCUE_MAX_YAW_RATE : errorAngleAbs;
rescueYaw *= errorAngleSign;