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

Fix curly brackets, comments and debug mistake

Browse source
This commit is contained in:
ctzsnooze 2024-11-06 23:35:08 +11:00
parent 5d8b4112f8
commit 6d99af0d50
1 changed files with 10 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -95,7 +95,8 @@ static posHoldState posHold = {
static gpsLocation_t currentTargetLocation = {0, 0, 0}; static gpsLocation_t currentTargetLocation = {0, 0, 0};
float autopilotAngle[2]; float autopilotAngle[2];
void resetPositionControlEFParams(earthFrame_t *efAxis) { void resetPositionControlEFParams(earthFrame_t *efAxis)
{
// at start only // at start only
pt1FilterInit(&efAxis->velocityLpf, posHold.pt1Gain); // Clear and initialise the filters pt1FilterInit(&efAxis->velocityLpf, posHold.pt1Gain); // Clear and initialise the filters
pt1FilterInit(&efAxis->accelerationLpf, posHold.pt1Gain); pt1FilterInit(&efAxis->accelerationLpf, posHold.pt1Gain);
@ -103,7 +104,8 @@ void resetPositionControlEFParams(earthFrame_t *efAxis) {
efAxis->integral = 0.0f; efAxis->integral = 0.0f;
} }
void resetPositionControl(gpsLocation_t initialTargetLocation) { void resetPositionControl(gpsLocation_t initialTargetLocation)
{
// from pos_hold.c when initiating position hold at target location // from pos_hold.c when initiating position hold at target location
currentTargetLocation = initialTargetLocation; currentTargetLocation = initialTargetLocation;
posHold.sticksActive = false; posHold.sticksActive = false;
@ -145,8 +147,8 @@ void resetAltitudeControl (void) {
altitudeI = 0.0f; altitudeI = 0.0f;
} }
void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep) { void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep)
{
const float altitudeErrorCm = targetAltitudeCm - getAltitudeCm(); const float altitudeErrorCm = targetAltitudeCm - getAltitudeCm();
const float altitudeP = altitudeErrorCm * altitudePidCoeffs.Kp; const float altitudeP = altitudeErrorCm * altitudePidCoeffs.Kp;
@ -279,7 +281,7 @@ bool positionControl(void)
// 'phase' after sticks stop, but before craft has stopped // 'phase' after sticks stop, but before craft has stopped
pidD *= 1.6f; // aribitrary D boost to stop more quickly when sticks are centered pidD *= 1.6f; // aribitrary D boost to stop more quickly when sticks are centered
if (velocity * velocityFiltered < 0.0f) { if (velocity * velocityFiltered < 0.0f) {
// when craft has nearly stopped moving, reset home and end the start phase // when an axis has nearly stopped moving, reset it and end it's start phase
resetLocation(efAxis, loopAxis); resetLocation(efAxis, loopAxis);
efAxis->isStarting = false; efAxis->isStarting = false;
} }
@ -308,7 +310,7 @@ bool positionControl(void)
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(pidD * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(pidD * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(pidA * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(pidA * 10));
} }
} } // end for loop
if (posHold.sticksActive) { if (posHold.sticksActive) {
// keep update sanity check distance while sticks are out // keep update sanity check distance while sticks are out
@ -340,8 +342,8 @@ bool positionControl(void)
// note: upsampling should really be done in earth frame, to avoid 10Hz wobbles if pilot yaws and the controller is applying significant pitch or roll // note: upsampling should really be done in earth frame, to avoid 10Hz wobbles if pilot yaws and the controller is applying significant pitch or roll
if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) { if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(posHold.efAxis[NS].distance)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(posHold.efAxis[EW].distance));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(posHold.efAxis[NS].pidSum * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(posHold.efAxis[EW].pidSum * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(autopilotAngle[AI_ROLL] * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(autopilotAngle[AI_ROLL] * 10));
} else { } else {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(posHold.efAxis[NS].distance)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(posHold.efAxis[NS].distance));