Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

Refactor

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2021-02-03 12:15:02 +01:00
parent ff46510b75
commit 0a4498dacc
3 changed files with 21 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.h
View file

@ -80,5 +80,6 @@ typedef enum {
DEBUG_SPM_VARIO, // Smartport master variometer
DEBUG_PCF8574,
DEBUG_DYNAMIC_GYRO_LPF,
DEBUG_AUTOLEVEL,
DEBUG_COUNT
} debugType_e;

2
src/main/fc/settings.yaml
View file

@ -1873,7 +1873,7 @@ groups:
min: 1
max: 16000
- name: fw_level_pitch_trim
description: "Pitch trim for self-leveling flight modes. In degrees"
description: "Pitch trim for self-leveling flight modes. In degrees. +5 means nose should be raised 5 deg from level"
default_value: "0"
field: fixedWingLevelTrim
min: -10

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

@ -537,16 +537,26 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
// Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);
angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);
}
DEBUG_SET(DEBUG_ALWAYS, 0, angleTarget * 10);
DEBUG_SET(DEBUG_ALWAYS, 1, fixedWingLevelTrim * 10);
DEBUG_SET(DEBUG_ALWAYS, 2, getEstimatedActualVelocity(Z));
//PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
if (axis == FD_PITCH && STATE(AIRPLANE)) {
DEBUG_SET(DEBUG_AUTOLEVEL, 0, angleTarget * 10);
DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10);
DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z));
//Apply level trim
angleTarget -= fixedWingLevelTrim;
DEBUG_SET(DEBUG_ALWAYS, 3, angleTarget * 10);
/*
* fixedWingLevelTrim has opposite sign to rcCommand.
* Positive rcCommand means nose should point downwards
* Negative rcCommand mean nose should point upwards
* This is counter intuitive and a natural way suggests that + should mean UP
* This is why fixedWingLevelTrim has opposite sign to rcCommand
* Positive fixedWingLevelTrim means nose should point upwards
* Negative fixedWingLevelTrim means nose should point downwards
*/
angleTarget -= fixedWingLevelTrim;
DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10);
}
const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
@ -1170,6 +1180,6 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
1.0f
);
DEBUG_SET(DEBUG_ALWAYS, 4, output / 100);
DEBUG_SET(DEBUG_AUTOLEVEL, 4, output / 100);
}