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-02-14 20:32:43 +01:00
commit 064a8eec7c
69 changed files with 1880 additions and 828 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -267,6 +267,10 @@ static int8_t loiterDirection(void) {
static void calculateVirtualPositionTarget_FW(float trackingPeriod)
{
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
return;
}
float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
@ -391,9 +395,14 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
static int32_t previousHeadingError;
static bool errorIsDecreasing;
static bool forceTurnDirection = false;
int32_t virtualTargetBearing;
// We have virtual position target, calculate heading error
int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
virtualTargetBearing = posControl.desiredState.yaw;
} else {
// We have virtual position target, calculate heading error
virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
}
/* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
@ -702,7 +711,7 @@ bool isFixedWingLandingDetected(void)
static int16_t fwLandSetPitchDatum;
const float sensitivity = navConfig()->general.land_detect_sensitivity / 5.0f;
timeMs_t currentTimeMs = millis();
const timeMs_t currentTimeMs = millis();
// Check horizontal and vertical velocities are low (cm/s)
bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (50.0f * sensitivity) &&
@ -727,9 +736,12 @@ bool isFixedWingLandingDetected(void)
DEBUG_SET(DEBUG_LANDING, 6, isRollAxisStatic);
DEBUG_SET(DEBUG_LANDING, 7, isPitchAxisStatic);
if (isRollAxisStatic && isPitchAxisStatic) {
// Probably landed, low horizontal and vertical velocities and no axis rotation in Roll and Pitch
timeMs_t safetyTimeDelay = 2000 + navConfig()->general.auto_disarm_delay;
return currentTimeMs - fwLandingTimerStartAt > safetyTimeDelay; // check conditions stable for 2s + optional extra delay
/* Probably landed, low horizontal and vertical velocities and no axis rotation in Roll and Pitch
* Conditions need to be held for fixed safety time + optional extra delay.
* Fixed time increased if velocities invalid to provide extra safety margin against false triggers */
const uint16_t safetyTime = posControl.flags.estAltStatus == EST_NONE || posControl.flags.estVelStatus == EST_NONE ? 5000 : 1000;
timeMs_t safetyTimeDelay = safetyTime + navConfig()->general.auto_disarm_delay;
return currentTimeMs - fwLandingTimerStartAt > safetyTimeDelay;
} else {
fixAxisCheck = false;
}
@ -743,8 +755,6 @@ bool isFixedWingLandingDetected(void)
*-----------------------------------------------------------*/
void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
{
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
if (posControl.flags.estAltStatus >= EST_USABLE) {
@ -756,6 +766,18 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
} else {
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
}
if (posControl.flags.estPosStatus >= EST_USABLE) { // Hold position if possible
applyFixedWingPositionController(currentTimeUs);
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]);
rcCommand[YAW] = 0;
} else {
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
}
}
/*-----------------------------------------------------------