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 
removed GPS Off box
implemented Disable GPS sensor fix logic condition
This commit is contained in:
Roman Lut 2022-12-10 19:26:08 +02:00
commit cb2d448911
379 changed files with 5433 additions and 3656 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -72,6 +72,7 @@ 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;
@ -240,11 +241,11 @@ void resetFixedWingPositionController(void)
static int8_t loiterDirection(void) {
int8_t dir = 1; //NAV_LOITER_RIGHT
if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) {
if (navConfig()->fw.loiter_direction == NAV_LOITER_LEFT) {
dir = -1;
}
if (pidProfile()->loiter_direction == NAV_LOITER_YAW) {
if (navConfig()->fw.loiter_direction == NAV_LOITER_YAW) {
if (rcCommand[YAW] < -250) {
loiterDirYaw = 1; //RIGHT //yaw is contrariwise
@ -285,30 +286,28 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
(distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
(distanceToActualTarget > 50.0f);
/* WP turn smoothing option - switches to loiter path around waypoint using navLoiterRadius.
/* WP turn smoothing with 2 options, 1: pass through WP, 2: cut inside turn missing WP
* Works for turns > 30 degs and < 160 degs.
* Option 1 switches to loiter path around waypoint using navLoiterRadius.
* Loiter centered on point inside turn at required distance from waypoint and
* on a bearing midway between current and next waypoint course bearings.
* Works for turns > 30 degs and < 120 degs.
* 2 options, 1 = pass through WP, 2 = cut inside turn missing WP */
* Option 2 simply uses a normal turn once the turn initiation point is reached */
int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle);
posControl.flags.wpTurnSmoothingActive = false;
if (waypointTurnAngle > 3000 && waypointTurnAngle < 12000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
// turnFactor adjusts start of loiter based on turn angle
float turnFactor = 0.0f;
if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
// turnStartFactor adjusts start of loiter based on turn angle
float turnStartFactor;
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) { // passes through WP
turnFactor = waypointTurnAngle / 6000.0f;
} else {
turnFactor = tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)); // cut inside turn missing WP
turnStartFactor = waypointTurnAngle / 6000.0f;
} else { // // cut inside turn missing WP
turnStartFactor = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 1.0f, 2.0f);
}
constrainf(turnFactor, 0.5f, 2.0f);
if (posControl.wpDistance < navLoiterRadius * turnFactor) {
// velXY provides additional turn initiation distance based on an assumed 1 second delayed turn response time
if (posControl.wpDistance < (posControl.actualState.velXY + navLoiterRadius * turnStartFactor)) {
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) {
int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.yaw + 18000);
float distToTurnCentre = navLoiterRadius;
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) {
distToTurnCentre = navLoiterRadius / cos_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f));
}
loiterCenterPos.x = posControl.activeWaypoint.pos.x + distToTurnCentre * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
loiterCenterPos.y = posControl.activeWaypoint.pos.y + distToTurnCentre * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
@ -316,8 +315,10 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// turn direction to next waypoint
loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right
needToCalculateCircularLoiter = posControl.flags.wpTurnSmoothingActive = true;
needToCalculateCircularLoiter = true;
}
posControl.flags.wpTurnSmoothingActive = true;
}
}
// We are closing in on a waypoint, calculate circular loiter if required
@ -396,26 +397,31 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
/* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
// only apply course tracking correction if target bearing error < 90 degs or when close to waypoint (within 10m)
if (ABS(wrap_18000(virtualTargetBearing - posControl.actualState.yaw)) < 9000 || posControl.wpDistance < 1000.0f) {
// courseVirtualCorrection initially used to determine current position relative to course line for later use
int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing);
float distToCourseLine = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
// tracking only active when certain distance and heading conditions are met
if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.yaw)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) {
int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw);
// captureFactor adjusts distance/heading sensitivity balance when closing in on course line.
// Closing distance threashold based on speed and an assumed 1 second response time.
float captureFactor = navCrossTrackError < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f;
// bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy
// initial courseCorrectionFactor based on distance to course line
float courseCorrectionFactor = constrainf(distToCourseLine / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
float courseCorrectionFactor = constrainf(captureFactor * navCrossTrackError / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
// course heading alignment factor
int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw);
float courseHeadingFactor = constrainf(sq(courseHeadingError / 18000.0f), 0.0f, 1.0f);
float courseHeadingFactor = constrainf(courseHeadingError / 18000.0f, 0.0f, 1.0f);
courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor;
// final courseCorrectionFactor combining distance and heading factors
courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f);
// final courseVirtualCorrection using max 80 deg heading adjustment toward course line
// final courseVirtualCorrection value
courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor;
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseVirtualCorrection);
}
@ -635,9 +641,11 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
* Then altitude is below landing slowdown min. altitude, enable final approach procedure
* TODO refactor conditions in this metod if logic is proven to be correct
*/
if (navStateFlags & NAV_CTL_LAND) {
if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (navGetCurrentActualPositionAndVelocity()->pos.z <= navConfig()->general.land_slowdown_minalt)) ||
((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) ) {
if (navStateFlags & NAV_CTL_LAND || STATE(LANDING_DETECTED)) {
int32_t finalAltitude = navConfig()->general.land_slowdown_minalt + posControl.rthState.homeTmpWaypoint.z;
if ((posControl.flags.estAltStatus >= EST_USABLE && navGetCurrentActualPositionAndVelocity()->pos.z <= finalAltitude) ||
(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();
@ -678,12 +686,11 @@ bool isFixedWingLandingDetected(void)
{
DEBUG_SET(DEBUG_LANDING, 4, 0);
static bool fixAxisCheck = false;
const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
// Basic condition to start looking for landing
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
|| FLIGHT_MODE(FAILSAFE_MODE)
|| (!navigationIsControllingThrottle() && throttleIsLow);
|| (!navigationIsControllingThrottle() && throttleStickIsLow());
if (!startCondition || posControl.flags.resetLandingDetector) {
return fixAxisCheck = posControl.flags.resetLandingDetector = false;
@ -693,13 +700,15 @@ bool isFixedWingLandingDetected(void)
static timeMs_t fwLandingTimerStartAt;
static int16_t fwLandSetRollDatum;
static int16_t fwLandSetPitchDatum;
const float sensitivity = navConfig()->general.land_detect_sensitivity / 5.0f;
timeMs_t currentTimeMs = millis();
// Check horizontal and vertical volocities are low (cm/s)
bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < 50.0f && ((posControl.actualState.velXY < 100.0f) || STATE(GPS_ESTIMATED_FIX));
// Check horizontal and vertical velocities are low (cm/s)
bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (50.0f * sensitivity) &&
( posControl.actualState.velXY < (100.0f * sensitivity) || STATE(GPS_ESTIMATED_FIX));
// Check angular rates are low (degs/s)
bool gyroCondition = averageAbsGyroRates() < 2.0f;
bool gyroCondition = averageAbsGyroRates() < (2.0f * sensitivity);
DEBUG_SET(DEBUG_LANDING, 2, velCondition);
DEBUG_SET(DEBUG_LANDING, 3, gyroCondition);
@ -712,13 +721,14 @@ bool isFixedWingLandingDetected(void)
fixAxisCheck = true;
fwLandingTimerStartAt = currentTimeMs;
} else {
bool isRollAxisStatic = ABS(fwLandSetRollDatum - attitude.values.roll) < 5;
bool isPitchAxisStatic = ABS(fwLandSetPitchDatum - attitude.values.pitch) < 5;
const uint8_t angleLimit = 5 * sensitivity;
bool isRollAxisStatic = ABS(fwLandSetRollDatum - attitude.values.roll) < angleLimit;
bool isPitchAxisStatic = ABS(fwLandSetPitchDatum - attitude.values.pitch) < angleLimit;
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()->fw.auto_disarm_delay;
timeMs_t safetyTimeDelay = 2000 + navConfig()->general.auto_disarm_delay;
return currentTimeMs - fwLandingTimerStartAt > safetyTimeDelay; // check conditions stable for 2s + optional extra delay
} else {
fixAxisCheck = false;
@ -815,3 +825,8 @@ int32_t navigationGetHeadingError(void)
{
return navHeadingError;
}
float navigationGetCrossTrackError(void)
{
return navCrossTrackError;
}