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

Merge pull request #3297 from shellixyz/fix_and_simplify_applyFixedWingPitchRollThrottleController

Browse source 
Fix and simplify applyFixedWingPitchRollThrottleController()
This commit is contained in:
Alberto García Hierro 2018-06-11 12:57:52 +01:00 • committed by GitHub
commit 99cbb3f8f0
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 18 additions and 45 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/navigation/navigation.c
View file

@ -2762,7 +2762,7 @@ rthState_e getStateOfForcedRTH(void)
bool navigationIsControllingThrottle(void)
{
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)) || (STATE(FIXED_WING) && (stateFlags & (NAV_CTL_POS)));
return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND));
}
bool navigationIsFlyingAutonomousMode(void)

61
src/main/navigation/navigation_fixedwing.c
View file

@ -412,28 +412,24 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
{
int16_t pitchCorrection = 0; // >0 climb, <0 dive
int16_t rollCorrection = 0; // >0 right, <0 left
int16_t throttleCorrection = 0; // raw throttle
int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle;
int16_t maxThrottleCorrection = navConfig()->fw.max_throttle - navConfig()->fw.cruise_throttle;
// Mix Pitch/Roll/Throttle
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
rollCorrection += posControl.rcAdjustment[ROLL];
// ROLL >0 right, <0 left
int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
}
if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
pitchCorrection += posControl.rcAdjustment[PITCH];
throttleCorrection += DECIDEGREES_TO_DEGREES(pitchCorrection) * navConfig()->fw.pitch_to_throttle;
// PITCH >0 dive, <0 climb
int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
int16_t throttleCorrection = DECIDEGREES_TO_DEGREES(pitchCorrection) * navConfig()->fw.pitch_to_throttle;
#ifdef NAV_FIXED_WING_LANDING
if (navStateFlags & NAV_CTL_LAND) {
/*
* During LAND we do not allow to raise THROTTLE when nose is up
* to reduce speed
*/
// During LAND we do not allow to raise THROTTLE when nose is up to reduce speed
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0);
} else {
#endif
@ -441,28 +437,13 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
#ifdef NAV_FIXED_WING_LANDING
}
#endif
}
// Speed controller - only apply in POS mode when NOT NAV_CTL_LAND
if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) {
throttleCorrection += applyFixedWingMinSpeedController(currentTimeUs);
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
}
// Speed controller - only apply in POS mode when NOT NAV_CTL_LAND
if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) {
throttleCorrection += applyFixedWingMinSpeedController(currentTimeUs);
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
}
// Limit and apply
if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
// PITCH correction is measured according to altitude: <0 - dive/lose altitude, >0 - climb/gain altitude
// PITCH angle is measured in opposite direction ( >0 - dive, <0 - climb)
pitchCorrection = constrain(pitchCorrection, -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
}
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
rollCorrection = constrain(rollCorrection, -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
}
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
rcCommand[THROTTLE] = constrain(correctedThrottleValue, motorConfig()->minthrottle, motorConfig()->maxthrottle);
}
@ -475,23 +456,15 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
if (navStateFlags & NAV_CTL_LAND) {
if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (navGetCurrentActualPositionAndVelocity()->pos.z <= navConfig()->general.land_slowdown_minalt)) ||
((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) ) {
/*
* Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
*/
// Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
rcCommand[THROTTLE] = motorConfig()->minthrottle;
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
/*
* Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
*/
// Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
rcCommand[ROLL] = 0;
/*
* Stabilize PITCH angle into shallow dive as specified by the
* nav_fw_land_dive_angle setting (default value is 2 - defined
* in navigation.c).
* PITCH angle is measured: >0 - dive, <0 - climb)
*/
// Stabilize PITCH angle into shallow dive as specified by the nav_fw_land_dive_angle setting (default value is 2 - defined in navigation.c).
rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navConfig()->fw.land_dive_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
}
}