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

Merge pull request #7364 from breadoven/abo_FW_emerg_landing_descent_rate

Browse source 
Fixed wing emergency landing vertical rate control addition
This commit is contained in:
Paweł Spychalski 2021-09-21 08:55:00 +02:00 • committed by GitHub
commit b51cb7739a
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 15 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -243,18 +243,18 @@ static int8_t loiterDirection(void) {
if (pidProfile()->loiter_direction == NAV_LOITER_YAW) { if (pidProfile()->loiter_direction == NAV_LOITER_YAW) {
if (rcCommand[YAW] < -250) { if (rcCommand[YAW] < -250) {
loiterDirYaw = 1; //RIGHT //yaw is contrariwise loiterDirYaw = 1; //RIGHT //yaw is contrariwise
} }
if (rcCommand[YAW] > 250) { if (rcCommand[YAW] > 250) {
loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c
} }
dir = loiterDirYaw; dir = loiterDirYaw;
} }
if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) { if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) {
dir *= -1; dir *= -1;
} }
@ -612,13 +612,21 @@ bool isFixedWingLandingDetected(void)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* FixedWing emergency landing * FixedWing emergency landing
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void applyFixedWingEmergencyLandingController(void) void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
{ {
// FIXME: Use altitude controller if available (similar to MC code)
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
if (posControl.flags.estAltStatus >= EST_USABLE) {
updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
applyFixedWingAltitudeAndThrottleController(currentTimeUs);
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]);
} else {
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
}
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------
@ -641,7 +649,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
applyFixedWingLaunchController(currentTimeUs); applyFixedWingLaunchController(currentTimeUs);
} }
else if (navStateFlags & NAV_CTL_EMERG) { else if (navStateFlags & NAV_CTL_EMERG) {
applyFixedWingEmergencyLandingController(); applyFixedWingEmergencyLandingController(currentTimeUs);
} }
else { else {
#ifdef NAV_FW_LIMIT_MIN_FLY_VELOCITY #ifdef NAV_FW_LIMIT_MIN_FLY_VELOCITY