Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 17:55:28 +03:00
Code Issues Wiki Activity

add z controller gain

Browse source
This commit is contained in:
JuliooCesarMDM 2021-12-26 20:37:01 -03:00
parent 893b03e653
commit 2d3fccde46
1 changed files with 6 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

7
src/main/navigation/navigation_multicopter.c
View file

@ -55,6 +55,9 @@
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Altitude controller for multicopter aircraft * Altitude controller for multicopter aircraft
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
#define Z_CONTROLLER_GAIN 0.25f
static int16_t rcCommandAdjustedThrottle; static int16_t rcCommandAdjustedThrottle;
static int16_t altHoldThrottleRCZero = 1500; static int16_t altHoldThrottleRCZero = 1500;
static pt1Filter_t altholdThrottleFilterState; static pt1Filter_t altholdThrottleFilterState;
@ -70,6 +73,7 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
float pos_desired_z = posControl.desiredState.pos.z; float pos_desired_z = posControl.desiredState.pos.z;
float targetVel = get_sqrt_controller(&alt_hold_sqrt_controller, &pos_desired_z, navGetCurrentActualPositionAndVelocity()->pos.z, (float)(1.0f / getGyroLooptime())); float targetVel = get_sqrt_controller(&alt_hold_sqrt_controller, &pos_desired_z, navGetCurrentActualPositionAndVelocity()->pos.z, (float)(1.0f / getGyroLooptime()));
//targetVel *= Z_CONTROLLER_GAIN;
posControl.desiredState.pos.z = pos_desired_z; posControl.desiredState.pos.z = pos_desired_z;
@ -117,7 +121,8 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)currentBatteryProfile->nav.mc.hover_throttle; const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)currentBatteryProfile->nav.mc.hover_throttle;
float accel_target_z = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0); float accel_target_z = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
//accel_target_z *= Z_CONTROLLER_GAIN;
/* /*
float accel_max_z = 0.0f; float accel_max_z = 0.0f;