Automatically pitch down in angle mode when throttle is bellow cruise…

… throttle
iNavFlight · May 28, 2018 · 542c81e · 542c81e
1 parent cde920e
commit 542c81e
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 3 deletions.
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -572,6 +572,10 @@ groups:
  field: appliedMixerPreset
  min: -1
  max: INT16_MAX
+ - name: min_throttle_down_pitch
+ field: minThrottleDownPitchAngle
+ min: 0
+ max: 450
 
  - name: PG_MOTOR_3D_CONFIG
  type: flight3DConfig_t

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
@@ -74,7 +74,8 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
  .yaw_jump_prevention_limit = 200,
  .platformType = PLATFORM_MULTIROTOR,
  .hasFlaps = false,
- .appliedMixerPreset = -1 //This flag is not available in CLI and used by Configurator only
+ .appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only
+ .minThrottleDownPitchAngle = 0
 );
 
 #ifdef BRUSHED_MOTORS

diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
@@ -59,6 +59,7 @@ typedef struct mixerConfig_s {
  uint8_t platformType;
  bool hasFlaps;
  int16_t appliedMixerPreset;
+ uint16_t minThrottleDownPitchAngle;
 } mixerConfig_t;
 
 PG_DECLARE(mixerConfig_t, mixerConfig);
@@ -97,4 +98,4 @@ void mixTable(void);
 void writeMotors(void);
 void processServoAutotrim(void);
 void stopMotors(void);
-void stopPwmAllMotors(void);
+void stopPwmAllMotors(void);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -413,7 +413,12 @@ static float calcHorizonRateMagnitude(void)
 static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude)
 {
  // This is ROLL/PITCH, run ANGLE/HORIZON controllers
- const float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
+ float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
+
+ // Automatically pitch down if the throttle is reduced bellow cruise throttle
+ if ((axis == FD_PITCH) && STATE(FIXED_WING) && FLIGHT_MODE(ANGLE_MODE))
+ angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->minThrottleDownPitchAngle);
+
  const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
 
  float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);