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

Use autopilotAngle in place of posHoldAngle

Browse source
This commit is contained in:
ctzsnooze 2024-10-18 23:52:25 +11:00
parent 9a80b75abe
commit 35d3006ecd
4 changed files with 10 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/main/flight/autopilot.c
View file

@ -77,7 +77,7 @@ static posHoldState posHold = {
};
static gpsLocation_t currentTargetLocation = {0, 0, 0};
float posHoldAngle[ANGLE_INDEX_COUNT];
float autopilotAngle[ANGLE_INDEX_COUNT];
static pt1Filter_t velocityPitchLpf;
static pt1Filter_t accelerationPitchLpf;
static pt1Filter_t velocityRollLpf;
@ -328,25 +328,25 @@ bool positionControl(bool useStickAdjustment, float deadband) {
// if done in pid.c, the same upsampler could be used for GPS and PosHold.
// if a deadband is set, and sticks are outside deadband, allow pilot control, otherwise hold position
posHoldAngle[AI_ROLL] = (useStickAdjustment && getRcDeflectionAbs(FD_ROLL) > deadband) ? 0.0f : pidSumRoll;
posHoldAngle[AI_PITCH] = (useStickAdjustment && getRcDeflectionAbs(FD_PITCH) > deadband) ? 0.0f : pidSumPitch;
autopilotAngle[AI_ROLL] = (useStickAdjustment && getRcDeflectionAbs(FD_ROLL) > deadband) ? 0.0f : pidSumRoll;
autopilotAngle[AI_PITCH] = (useStickAdjustment && getRcDeflectionAbs(FD_PITCH) > deadband) ? 0.0f : pidSumPitch;
// note:
// if FLIGHT_MODE(POS_HOLD_MODE):
// posHoldAngle[] is added to angle setpoint in pid.c, in degrees
// autopilotAngle[] is added to angle setpoint in pid.c, in degrees
// stick angle setpoint forced to zero within the same deadband via rc.c
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(normalisedErrorAngle));
if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(-distanceRoll));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(posHoldAngle[AI_ROLL] * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(autopilotAngle[AI_ROLL] * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(rollP * 10)); // degrees*10
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(posHold.rollI * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(rollD * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(rollJ * 10));
} else {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(-distancePitch));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(posHoldAngle[AI_PITCH] * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(autopilotAngle[AI_PITCH] * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(pitchP * 10)); // degrees*10
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(posHold.pitchI * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(pitchD * 10));

2
src/main/flight/autopilot.h
View file

@ -21,7 +21,7 @@
#include "flight/pid.h"
#include "io/gps.h"
extern float posHoldAngle[ANGLE_INDEX_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
extern float autopilotAngle[ANGLE_INDEX_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
void autopilotInit(const autopilotConfig_t *config);
void resetAltitudeControl(void);

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

@ -569,7 +569,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
angleTarget += gpsRescueAngle[axis] / 100.0f; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch
#endif
#ifdef USE_POS_HOLD_MODE
angleTarget += posHoldAngle[axis]; // posHoldAngle is in degrees, stepped at 10Hz
angleTarget += autopilotAngle[axis]; // autopilotAngle is in degrees, stepped at 10Hz
#endif
const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f; // stepped at 500hz with some 4ms flat spots

4
src/main/flight/pos_hold.c
View file

@ -85,8 +85,8 @@ void posHoldNewGpsData(void) {
posHold.posHoldIsOK = positionControl(posHold.useStickAdjustment, posHold.deadband);
}
} else {
posHoldAngle[AI_PITCH] = 0.0f;
posHoldAngle[AI_ROLL] = 0.0f;
autopilotAngle[AI_PITCH] = 0.0f;
autopilotAngle[AI_ROLL] = 0.0f;
}
}