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

Merge branch 'iNavFlight:master' into strobeblink-refactor

Browse source
This commit is contained in:
Marcelo Bezerra 2023-01-18 20:43:30 +01:00 • committed by GitHub
commit fddeb789ab
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 25 additions and 38 deletions
Download patch file Download diff file Expand all files Collapse all files

49
src/main/navigation/navigation.c
View file

@ -257,7 +257,6 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
void calculateInitialHoldPosition(fpVector3_t * pos);
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance);
void calculateNewCruiseTarget(fpVector3_t * origin, int32_t course, int32_t distance);
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing);
bool isWaypointAltitudeReached(void);
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
@ -1059,7 +1058,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState)
{
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
UNUSED(previousState);
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
@ -1068,19 +1067,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
} // Switch to IDLE if we do not have an healty position. Try the next iteration.
if (!(prevFlags & NAV_CTL_POS)) {
resetPositionController();
}
resetPositionController();
posControl.cruise.course = posControl.actualState.cog; // Store the course to follow
posControl.cruise.previousCourse = posControl.cruise.course;
posControl.cruise.lastCourseAdjustmentTime = 0;
return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state
return NAV_FSM_EVENT_SUCCESS; // Go to NAV_STATE_COURSE_HOLD_IN_PROGRESS state
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState)
{
UNUSED(previousState);
const timeMs_t currentTimeMs = millis();
if (checkForPositionSensorTimeout()) {
@ -1097,30 +1096,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
// User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
if (posControl.flags.isAdjustingHeading) {
timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastCourseAdjustmentTime;
if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
float centidegsPerIteration = rateTarget * timeDifference * 0.001f;
float centidegsPerIteration = rateTarget * MS2S(timeDifference);
posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration);
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course));
posControl.cruise.lastCourseAdjustmentTime = currentTimeMs;
}
if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000)
} else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) {
posControl.cruise.previousCourse = posControl.cruise.course;
uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm]
if ((previousState == NAV_STATE_COURSE_HOLD_INITIALIZE) || (previousState == NAV_STATE_COURSE_HOLD_ADJUSTING)
|| (previousState == NAV_STATE_CRUISE_INITIALIZE) || (previousState == NAV_STATE_CRUISE_ADJUSTING)
|| posControl.flags.isAdjustingHeading) {
calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.course, distance);
DEBUG_SET(DEBUG_CRUISE, 2, 1);
} else if (calculateDistanceToDestination(&posControl.cruise.targetPos) <= (navConfig()->fw.loiter_radius * 1.10f)) { //10% margin
calculateNewCruiseTarget(&posControl.cruise.targetPos, posControl.cruise.course, distance);
DEBUG_SET(DEBUG_CRUISE, 2, 2);
}
setDesiredPosition(&posControl.cruise.targetPos, posControl.cruise.course, NAV_POS_UPDATE_XY);
setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE;
}
@ -1139,7 +1125,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n
resetPositionController();
return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state
return NAV_FSM_EVENT_SUCCESS; // go back to NAV_STATE_COURSE_HOLD_IN_PROGRESS state
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState)
@ -2812,13 +2798,6 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t d
farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
}
void calculateNewCruiseTarget(fpVector3_t * origin, int32_t course, int32_t distance)
{
origin->x = origin->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(course));
origin->y = origin->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(course));
origin->z = origin->z;
}
/*-----------------------------------------------------------
* NAV land detector
*-----------------------------------------------------------*/
@ -3755,13 +3734,13 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
// PH has priority over COURSE_HOLD
// CRUISE has priority on AH
if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) {
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)))
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) {
return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
}
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold))
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold)) {
return NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD;
}
}
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {

13
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) {

1
src/main/navigation/navigation_private.h
View file

@ -323,7 +323,6 @@ typedef struct {
} rthSanityChecker_t;
typedef struct {
fpVector3_t targetPos;
int32_t course;
int32_t previousCourse;
timeMs_t lastCourseAdjustmentTime;