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

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

Browse source
This commit is contained in:
breadoven 2021-10-28 20:31:47 +01:00
commit 9ce4e2758f
261 changed files with 4827 additions and 4478 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -49,6 +49,8 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "programming/logic_condition.h"
#include "rx/rx.h"
#include "sensors/battery.h"
@ -271,10 +273,12 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// Limit minimum forward velocity to 1 m/s
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius);
// If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
#define TAN_15DEG 0.26795f
bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
&& (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
&& (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG))
&& (distanceToActualTarget > 50.0f)
&& !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
@ -283,8 +287,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// We are closing in on a waypoint, calculate circular loiter
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f);
float loiterTargetX = posControl.desiredState.pos.x + navConfig()->fw.loiter_radius * cos_approx(loiterAngle);
float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle);
float loiterTargetX = posControl.desiredState.pos.x + navLoiterRadius * cos_approx(loiterAngle);
float loiterTargetY = posControl.desiredState.pos.y + navLoiterRadius * sin_approx(loiterAngle);
// We have temporary loiter target. Recalculate distance and position error
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
@ -612,13 +616,21 @@ bool isFixedWingLandingDetected(void)
/*-----------------------------------------------------------
* 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[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] = 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 +653,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
applyFixedWingLaunchController(currentTimeUs);
}
else if (navStateFlags & NAV_CTL_EMERG) {
applyFixedWingEmergencyLandingController();
applyFixedWingEmergencyLandingController(currentTimeUs);
}
else {
#ifdef NAV_FW_LIMIT_MIN_FLY_VELOCITY