Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-12 19:10:27 +03:00
Code Issues Wiki Activity

Use nav bank angle for mc ATTI mode

Browse source
This commit is contained in:
breadoven 2025-05-15 09:30:11 +01:00 • committed by Ray Morris
parent 4ce017e202
commit bbd57eb248
1 changed files with 30 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -596,38 +596,45 @@ int16_t angleFreefloatDeadband(int16_t deadband, flight_dynamics_index_t axis)
static float computePidLevelTarget(flight_dynamics_index_t axis) {
// This is ROLL/PITCH, run ANGLE/HORIZON controllers
// Limit max bank angle for multirotor during Nav mode Angle controlled position adjustment
uint16_t maxBankAngle = STATE(MULTIROTOR) && navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI && isAdjustingPosition() ?
DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle) : pidProfile()->max_angle_inclination[axis];
#ifdef USE_PROGRAMMING_FRAMEWORK
float angleTarget = pidRcCommandToAngle(getRcCommandOverride(rcCommand, axis), pidProfile()->max_angle_inclination[axis]);
float angleTarget = pidRcCommandToAngle(getRcCommandOverride(rcCommand, axis), maxBankAngle);
#else
float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
float angleTarget = pidRcCommandToAngle(rcCommand[axis], maxBankAngle);
#endif
// Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
if (STATE(AIRPLANE)) {
// Automatically pitch down if the throttle is manually controlled and reduced below cruise throttle
#ifdef USE_FW_AUTOLAND
if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !FLIGHT_MODE(NAV_FW_AUTOLAND)) {
if (axis == FD_PITCH && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !FLIGHT_MODE(NAV_FW_AUTOLAND)) {
#else
if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
if (axis == FD_PITCH && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
#endif
angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, navConfig()->fw.minThrottleDownPitchAngle);
}
angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, navConfig()->fw.minThrottleDownPitchAngle);
}
//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));
//PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
if (axis == FD_PITCH) {
DEBUG_SET(DEBUG_AUTOLEVEL, 0, angleTarget * 10);
DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10);
DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z));
/*
* 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 -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);
DEBUG_SET(DEBUG_AUTOLEVEL, 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 -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);
DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10);
}
}
return angleTarget;