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

Merge remote-tracking branch 'upstream/master' into abo_landing_detection

Browse source
This commit is contained in:
breadoven 2021-07-15 08:30:14 +01:00
commit 566d971026
177 changed files with 8268 additions and 1811 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -53,6 +53,8 @@
#include "rx/rx.h"
#include "sensors/battery.h"
// Base frequencies for smoothing pitch and roll
#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f
#define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f
@ -134,10 +136,10 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
// velocity error. We use PID controller on altitude error and calculate desired pitch angle
// Update energies
const float demSPE = (posControl.desiredState.pos.z / 100.0f) * GRAVITY_MSS;
const float demSPE = (posControl.desiredState.pos.z * 0.01f) * GRAVITY_MSS;
const float demSKE = 0.0f;
const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z / 100.0f) * GRAVITY_MSS;
const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z * 0.01f) * GRAVITY_MSS;
const float estSKE = 0.0f;
// speedWeight controls balance between potential and kinetic energy used for pitch controller
@ -236,13 +238,28 @@ void resetFixedWingPositionController(void)
static int8_t loiterDirection(void) {
int8_t dir = 1; //NAV_LOITER_RIGHT
if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) dir = -1;
if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) {
dir = -1;
}
if (pidProfile()->loiter_direction == NAV_LOITER_YAW) {
if (rcCommand[YAW] < -250) loiterDirYaw = 1; //RIGHT //yaw is contrariwise
if (rcCommand[YAW] > 250) loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c
if (rcCommand[YAW] < -250) {
loiterDirYaw = 1; //RIGHT //yaw is contrariwise
}
if (rcCommand[YAW] > 250) {
loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c
}
dir = loiterDirYaw;
}
if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) dir *= -1;
if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) {
dir *= -1;
}
return dir;
}
@ -251,7 +268,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
float distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
float distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY));
// Limit minimum forward velocity to 1 m/s
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
@ -274,7 +291,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// We have temporary loiter target. Recalculate distance and position error
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
posErrorY = loiterTargetY - navGetCurrentActualPositionAndVelocity()->pos.y;
distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY));
}
// Calculate virtual waypoint
@ -318,7 +335,7 @@ float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingErr
-limit,
limit,
yawPidFlags
) / 100.0f;
) * 0.01f;
DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
@ -485,18 +502,18 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs
if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
// Unfiltered throttle correction outside of pitch deadband
return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
return DECIDEGREES_TO_DEGREES(pitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
}
else {
// Filtered throttle correction inside of pitch deadband
return DECIDEGREES_TO_DEGREES(filteredPitch) * navConfig()->fw.pitch_to_throttle;
return DECIDEGREES_TO_DEGREES(filteredPitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
}
}
void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
{
int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle;
int16_t maxThrottleCorrection = navConfig()->fw.max_throttle - navConfig()->fw.cruise_throttle;
int16_t minThrottleCorrection = currentBatteryProfile->nav.fw.min_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
int16_t maxThrottleCorrection = currentBatteryProfile->nav.fw.max_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
// ROLL >0 right, <0 left
@ -531,15 +548,16 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
}
uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
// Manual throttle increase
if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) {
if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95)
correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - navConfig()->fw.cruise_throttle);
else
if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){
correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
} else {
correctedThrottleValue = motorConfig()->maxthrottle;
isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > navConfig()->fw.cruise_throttle);
}
isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle);
} else {
isAutoThrottleManuallyIncreased = false;
}
@ -645,7 +663,7 @@ void applyFixedWingEmergencyLandingController(void)
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[THROTTLE] = failsafeConfig()->failsafe_throttle;
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
}
/*-----------------------------------------------------------
@ -682,24 +700,28 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
// Motor has been stopped by user. Update target altitude and bypass navigation pitch/throttle control
resetFixedWingAltitudeController();
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
} else
} else {
applyFixedWingAltitudeAndThrottleController(currentTimeUs);
}
}
if (navStateFlags & NAV_CTL_POS)
if (navStateFlags & NAV_CTL_POS) {
applyFixedWingPositionController(currentTimeUs);
}
} else {
posControl.rcAdjustment[PITCH] = 0;
posControl.rcAdjustment[ROLL] = 0;
}
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition)
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition) {
rcCommand[ROLL] = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
}
//if (navStateFlags & NAV_CTL_YAW)
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS))
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
applyFixedWingPitchRollThrottleController(navStateFlags, currentTimeUs);
}
}
}