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

Merge branch 'master' of https://github.com/RomanLut/inav into submit-gps-fix-estimation

Browse source
This commit is contained in:
Roman Lut 2023-10-01 00:05:07 +02:00
commit e4453c36d8
233 changed files with 3760 additions and 1918 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -39,6 +39,7 @@
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/mixer_profile.h"
#include "fc/config.h"
#include "fc/controlrate_profile.h"
@ -71,7 +72,6 @@ static bool isRollAdjustmentValid = false;
static bool isYawAdjustmentValid = false;
static float throttleSpeedAdjustment = 0;
static bool isAutoThrottleManuallyIncreased = false;
static int32_t navHeadingError;
static float navCrossTrackError;
static int8_t loiterDirYaw = 1;
bool needToCalculateCircularLoiter;
@ -294,6 +294,10 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
needToCalculateCircularLoiter = isNavHoldPositionActive() &&
(distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
(distanceToActualTarget > 50.0f);
//if vtol landing is required, fly straight to homepoint
if ((posControl.navState == NAV_STATE_RTH_HEAD_HOME) && navigationRTHAllowsLanding() && checkMixerATRequired(MIXERAT_REQUEST_LAND)){
needToCalculateCircularLoiter = false;
}
/* WP turn smoothing with 2 options, 1: pass through WP, 2: cut inside turn missing WP
* Works for turns > 30 degs and < 160 degs.
@ -445,7 +449,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
* Calculate NAV heading error
* Units are centidegrees
*/
navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog);
int32_t navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog);
// Forced turn direction
// If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg
@ -647,7 +651,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
isAutoThrottleManuallyIncreased = false;
}
rcCommand[THROTTLE] = constrain(correctedThrottleValue, getThrottleIdleValue(), motorConfig()->maxthrottle);
rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false);
}
#ifdef NAV_FIXED_WING_LANDING
@ -662,7 +666,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
(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
rcCommand[THROTTLE] = getThrottleIdleValue();
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
// Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
@ -762,7 +765,7 @@ bool isFixedWingLandingDetected(void)
*-----------------------------------------------------------*/
void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
{
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
if (posControl.flags.estAltStatus >= EST_USABLE) {
// target min descent rate 10m above takeoff altitude
@ -851,11 +854,6 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
}
}
int32_t navigationGetHeadingError(void)
{
return navHeadingError;
}
float navigationGetCrossTrackError(void)
{
return navCrossTrackError;