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

add gps rescue mode

Browse source
This commit is contained in:
s0up 2018-05-15 14:32:03 -07:00 • committed by Diego Basch
parent 43c706fc95
commit ac6b8088c9
24 changed files with 653 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/main/flight/pid.c
View file

@ -47,6 +47,7 @@
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/gps_rescue.h"
#include "flight/mixer.h"
#include "io/gps.h"
@ -422,9 +423,12 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit
// calculate error angle and limit the angle to the max inclination
// rcDeflection is in range [-1.0, 1.0]
float angle = pidProfile->levelAngleLimit * getRcDeflection(axis);
#ifdef USE_GPS_RESCUE
angle += gpsRescueAngle[axis] / 100; // ANGLE IS IN CENTIDEGREES
#endif
angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
if (FLIGHT_MODE(ANGLE_MODE)) {
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) {
// ANGLE mode - control is angle based
currentPidSetpoint = errorAngle * levelGain;
} else {
@ -499,7 +503,7 @@ static void detectAndSetCrashRecovery(
{
// if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
// no point in trying to recover if the crash is so severe that the gyro overflows
if (crash_recovery && !gyroOverflowDetected()) {
if ((crash_recovery || FLIGHT_MODE(GPS_RESCUE_MODE)) && !gyroOverflowDetected()) {
if (ARMING_FLAG(ARMED)) {
if (getMotorMixRange() >= 1.0f && !inCrashRecoveryMode
&& ABS(delta) > crashDtermThreshold
@ -574,7 +578,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
}
// Yaw control is GYRO based, direct sticks control is applied to rate PID
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) && axis != YAW) {
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
}