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

Fix and simplify applyFixedWingPitchRollThrottleController()

Browse source 
Should only control pitch and throttle when nav stateFlags & NAV_CTL_ALT
Needed for upcoming CRUISE 2D mode
This commit is contained in:
Michel Pastor 2018-05-31 01:17:00 +02:00
parent 83bb1b8d3a
commit b2ef281619
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

@ -2648,7 +2648,7 @@ rthState_e getStateOfForcedRTH(void)
bool navigationIsControllingThrottle(void) bool navigationIsControllingThrottle(void)
{ {
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); 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) bool navigationIsFlyingAutonomousMode(void)

51
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) 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 minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle;
int16_t maxThrottleCorrection = navConfig()->fw.max_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)) { 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)) { if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
pitchCorrection += posControl.rcAdjustment[PITCH]; // PITCH >0 dive, <0 climb
throttleCorrection += DECIDEGREES_TO_DEGREES(pitchCorrection) * navConfig()->fw.pitch_to_throttle; 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 #ifdef NAV_FIXED_WING_LANDING
if (navStateFlags & NAV_CTL_LAND) { 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); throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0);
} else { } else {
#endif #endif
@ -441,7 +437,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
#ifdef NAV_FIXED_WING_LANDING #ifdef NAV_FIXED_WING_LANDING
} }
#endif #endif
}
// Speed controller - only apply in POS mode when NOT NAV_CTL_LAND // Speed controller - only apply in POS mode when NOT NAV_CTL_LAND
if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) { if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) {
@ -449,20 +444,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection); 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); 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); rcCommand[THROTTLE] = constrain(correctedThrottleValue, motorConfig()->minthrottle, motorConfig()->maxthrottle);
} }
@ -475,23 +456,15 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
if (navStateFlags & NAV_CTL_LAND) { if (navStateFlags & NAV_CTL_LAND) {
if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (navGetCurrentActualPositionAndVelocity()->pos.z <= navConfig()->general.land_slowdown_minalt)) || 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)) ) { ((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; rcCommand[THROTTLE] = motorConfig()->minthrottle;
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); 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; 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).
* 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)
*/
rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navConfig()->fw.land_dive_angle), pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navConfig()->fw.land_dive_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
} }
} }