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

first cut

Browse source
This commit is contained in:
breadoven 2022-12-19 00:06:01 +00:00
parent 5d84300545
commit a366ae4671
5 changed files with 69 additions and 73 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -256,9 +256,9 @@ static void clearJumpCounters(void);
static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint); static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos); static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
void calculateInitialHoldPosition(fpVector3_t * pos); void calculateInitialHoldPosition(fpVector3_t * pos);
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance);
void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance); void calculateNewCruiseTarget(fpVector3_t * origin, int32_t course, int32_t distance);
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointYaw); static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing);
bool isWaypointAltitudeReached(void); bool isWaypointAltitudeReached(void);
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
static navigationFSMEvent_t nextForNonGeoStates(void); static navigationFSMEvent_t nextForNonGeoStates(void);
@ -1072,9 +1072,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
resetPositionController(); resetPositionController();
} }
posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow posControl.cruise.course = posControl.actualState.cog; // Store the course to follow
posControl.cruise.previousYaw = posControl.cruise.yaw; posControl.cruise.previousCourse = posControl.cruise.course;
posControl.cruise.lastYawAdjustmentTime = 0; posControl.cruise.lastCourseAdjustmentTime = 0;
return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state
} }
@ -1096,31 +1096,31 @@ 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 // User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
if (posControl.flags.isAdjustingHeading) { if (posControl.flags.isAdjustingHeading) {
timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime; 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 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 * timeDifference * 0.001f;
posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration); posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration);
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw)); DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course));
posControl.cruise.lastYawAdjustmentTime = currentTimeMs; posControl.cruise.lastCourseAdjustmentTime = currentTimeMs;
} }
if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000) if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000)
posControl.cruise.previousYaw = posControl.cruise.yaw; posControl.cruise.previousCourse = posControl.cruise.course;
uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm] 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) if ((previousState == NAV_STATE_COURSE_HOLD_INITIALIZE) || (previousState == NAV_STATE_COURSE_HOLD_ADJUSTING)
|| (previousState == NAV_STATE_CRUISE_INITIALIZE) || (previousState == NAV_STATE_CRUISE_ADJUSTING) || (previousState == NAV_STATE_CRUISE_INITIALIZE) || (previousState == NAV_STATE_CRUISE_ADJUSTING)
|| posControl.flags.isAdjustingHeading) { || posControl.flags.isAdjustingHeading) {
calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance); calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.course, distance);
DEBUG_SET(DEBUG_CRUISE, 2, 1); DEBUG_SET(DEBUG_CRUISE, 2, 1);
} else if (calculateDistanceToDestination(&posControl.cruise.targetPos) <= (navConfig()->fw.loiter_radius * 1.10f)) { //10% margin } else if (calculateDistanceToDestination(&posControl.cruise.targetPos) <= (navConfig()->fw.loiter_radius * 1.10f)) { //10% margin
calculateNewCruiseTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance); calculateNewCruiseTarget(&posControl.cruise.targetPos, posControl.cruise.course, distance);
DEBUG_SET(DEBUG_CRUISE, 2, 2); DEBUG_SET(DEBUG_CRUISE, 2, 2);
} }
setDesiredPosition(&posControl.cruise.targetPos, posControl.cruise.yaw, NAV_POS_UPDATE_XY); setDesiredPosition(&posControl.cruise.targetPos, posControl.cruise.course, NAV_POS_UPDATE_XY);
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
@ -1132,8 +1132,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n
// User is rolling, changing manually direction. Wait until it is done and then restore CRUISE // User is rolling, changing manually direction. Wait until it is done and then restore CRUISE
if (posControl.flags.isAdjustingPosition) { if (posControl.flags.isAdjustingPosition) {
posControl.cruise.yaw = posControl.actualState.yaw; //store current heading posControl.cruise.course = posControl.actualState.cog; //store current course
posControl.cruise.lastYawAdjustmentTime = millis(); posControl.cruise.lastCourseAdjustmentTime = millis();
return NAV_FSM_EVENT_NONE; // reprocess the state return NAV_FSM_EVENT_NONE; // reprocess the state
} }
@ -1222,7 +1222,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
// Spiral climb centered at xy of RTH activation // Spiral climb centered at xy of RTH activation
calculateInitialHoldPosition(&targetHoldPos); calculateInitialHoldPosition(&targetHoldPos);
} else { } else {
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away Linear climb calculateFarAwayTarget(&targetHoldPos, posControl.actualState.cog, 100000.0f); // 1km away Linear climb
} }
} else { } else {
// Multicopter, hover and climb // Multicopter, hover and climb
@ -1341,7 +1341,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; // procede to home after final trackback point return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; // procede to home after final trackback point
} }
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.yaw)) { if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
posControl.activeRthTBPointIndex--; posControl.activeRthTBPointIndex--;
if (posControl.rthTBWrapAroundCounter > -1 && posControl.activeRthTBPointIndex < 0) { if (posControl.rthTBWrapAroundCounter > -1 && posControl.activeRthTBPointIndex < 0) {
@ -1377,7 +1377,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
if (isWaypointReached(tmpHomePos, 0)) { if (isWaypointReached(tmpHomePos, 0)) {
// Successfully reached position target - update XYZ-position // Successfully reached position target - update XYZ-position
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
} else { } else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY); setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
@ -1408,13 +1408,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
// If position ok OR within valid timeout - continue // If position ok OR within valid timeout - continue
// Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) { if ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
resetLandingDetector(); // force reset landing detector just in case resetLandingDetector(); // force reset landing detector just in case
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
} else { } else {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
} }
@ -1629,7 +1629,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
case NAV_WP_ACTION_HOLD_TIME: case NAV_WP_ACTION_HOLD_TIME:
case NAV_WP_ACTION_WAYPOINT: case NAV_WP_ACTION_WAYPOINT:
case NAV_WP_ACTION_LAND: case NAV_WP_ACTION_LAND:
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.yaw)) { if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
} }
else { else {
@ -2140,19 +2140,14 @@ void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroun
// the offset from the fake to the actual yaw and apply the same rotation // the offset from the fake to the actual yaw and apply the same rotation
// to the home point. // to the home point.
int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw; int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw;
posControl.rthState.homePosition.heading += fakeToRealYawOffset;
posControl.rthState.homePosition.heading = wrap_36000(posControl.rthState.homePosition.heading);
posControl.rthState.homePosition.yaw += fakeToRealYawOffset;
if (posControl.rthState.homePosition.yaw < 0) {
posControl.rthState.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360);
}
if (posControl.rthState.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) {
posControl.rthState.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360);
}
posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING; posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
} }
/* Use course over ground for fixed wing nav "heading" when valid - TODO use heading and cog as specifically required for FW and MR */
posControl.actualState.yaw = newHeading; posControl.actualState.yaw = newHeading;
posControl.actualState.cog = newGroundCourse; // currently only used for OSD display posControl.actualState.cog = newGroundCourse;
posControl.flags.estHeadingStatus = newEstHeading; posControl.flags.estHeadingStatus = newEstHeading;
/* Precompute sin/cos of yaw angle */ /* Precompute sin/cos of yaw angle */
@ -2207,7 +2202,7 @@ int32_t calculateBearingBetweenLocalPositions(const fpVector3_t * startPos, cons
return calculateBearingFromDelta(deltaX, deltaY); return calculateBearingFromDelta(deltaX, deltaY);
} }
bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos) bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos) // NOT USED ANYWHERE
{ {
if (posControl.flags.estPosStatus == EST_NONE || if (posControl.flags.estPosStatus == EST_NONE ||
posControl.flags.estHeadingStatus == EST_NONE) { posControl.flags.estHeadingStatus == EST_NONE) {
@ -2254,9 +2249,9 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Check if waypoint is/was reached. * Check if waypoint is/was reached.
* waypointYaw stores initial bearing to waypoint * waypointBearing stores initial bearing to waypoint
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointYaw) static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing)
{ {
posControl.wpDistance = calculateDistanceToDestination(waypointPos); posControl.wpDistance = calculateDistanceToDestination(waypointPos);
@ -2275,7 +2270,7 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w
// Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw // Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw
// Same method for turn smoothing option but relative bearing set at 60 degrees // Same method for turn smoothing option but relative bearing set at 60 degrees
uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000; uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000;
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > relativeBearing) { if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearing) {
return true; return true;
} }
} }
@ -2400,7 +2395,7 @@ bool validateRTHSanityChecker(void)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Reset home position to current position * Reset home position to current position
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags) void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags)
{ {
// XY-position // XY-position
if ((useMask & NAV_POS_UPDATE_XY) != 0) { if ((useMask & NAV_POS_UPDATE_XY) != 0) {
@ -2426,7 +2421,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t
// Heading // Heading
if ((useMask & NAV_POS_UPDATE_HEADING) != 0) { if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
// Heading // Heading
posControl.rthState.homePosition.yaw = yaw; posControl.rthState.homePosition.heading = heading;
if (homeFlags & NAV_HOME_VALID_HEADING) { if (homeFlags & NAV_HOME_VALID_HEADING) {
posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING; posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
} else { } else {
@ -2677,7 +2672,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
if (posControl.activeRthTBPointIndex < 0) { if (posControl.activeRthTBPointIndex < 0) {
saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(50); saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(50);
previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw); previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.cog);
previousTBTripDist = posControl.totalTripDistance; previousTBTripDist = posControl.totalTripDistance;
} else { } else {
// Minimum distance increment between course change track points when GPS course valid - set to 10m // Minimum distance increment between course change track points when GPS course valid - set to 10m
@ -2808,17 +2803,17 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
} }
} }
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance) void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance)
{ {
farAwayPos->x = navGetCurrentActualPositionAndVelocity()->pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw)); farAwayPos->x = navGetCurrentActualPositionAndVelocity()->pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(bearing));
farAwayPos->y = navGetCurrentActualPositionAndVelocity()->pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw)); farAwayPos->y = navGetCurrentActualPositionAndVelocity()->pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(bearing));
farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z; farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
} }
void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance) void calculateNewCruiseTarget(fpVector3_t * origin, int32_t course, int32_t distance)
{ {
origin->x = origin->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw)); origin->x = origin->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(course));
origin->y = origin->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw)); origin->y = origin->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(course));
origin->z = origin->z; origin->z = origin->z;
} }
@ -3388,18 +3383,18 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos) static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
{ {
// Calculate bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints) // Calculate bearing towards waypoint and store it in waypoint bearing parameter (this will further be used to detect missed waypoints)
if (isWaypointNavTrackingActive() && !(posControl.activeWaypoint.pos.x == pos->x && posControl.activeWaypoint.pos.y == pos->y)) { if (isWaypointNavTrackingActive() && !(posControl.activeWaypoint.pos.x == pos->x && posControl.activeWaypoint.pos.y == pos->y)) {
posControl.activeWaypoint.yaw = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos); posControl.activeWaypoint.bearing = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos);
} else { } else {
posControl.activeWaypoint.yaw = calculateBearingToDestination(pos); posControl.activeWaypoint.bearing = calculateBearingToDestination(pos);
} }
posControl.activeWaypoint.nextTurnAngle = -1; // no turn angle set (-1), will be set by WP mode as required posControl.activeWaypoint.nextTurnAngle = -1; // no turn angle set (-1), will be set by WP mode as required
posControl.activeWaypoint.pos = *pos; posControl.activeWaypoint.pos = *pos;
// Set desired position to next waypoint (XYZ-controller) // Set desired position to next waypoint (XYZ-controller)
setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.bearing, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
} }
geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag) geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag)
@ -3417,7 +3412,7 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
fpVector3_t posNextWp; fpVector3_t posNextWp;
if (getLocalPosNextWaypoint(&posNextWp)) { if (getLocalPosNextWaypoint(&posNextWp)) {
int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp); int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp);
posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.yaw); posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing);
} }
} }
} }
@ -3855,7 +3850,7 @@ bool navigationPositionEstimateIsHealthy(void)
navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
{ {
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE))); const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)));
if (usedBypass) { if (usedBypass) {
*usedBypass = false; *usedBypass = false;
} }
@ -4340,7 +4335,7 @@ bool abortLaunchAllowed(void)
int32_t navigationGetHomeHeading(void) int32_t navigationGetHomeHeading(void)
{ {
return posControl.rthState.homePosition.yaw; return posControl.rthState.homePosition.heading;
} }
// returns m/s // returns m/s
@ -4363,5 +4358,5 @@ bool isAdjustingHeading(void) {
} }
int32_t getCruiseHeadingAdjustment(void) { int32_t getCruiseHeadingAdjustment(void) {
return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw); return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse);
} }

7
src/main/navigation/navigation.h
View file

@ -395,11 +395,12 @@ extern radar_pois_t radar_pois[RADAR_MAX_POIS];
typedef struct { typedef struct {
fpVector3_t pos; fpVector3_t pos;
int32_t yaw; // centidegrees int32_t heading; // centidegrees
int32_t bearing; // centidegrees
int32_t nextTurnAngle; // centidegrees int32_t nextTurnAngle; // centidegrees
} navWaypointPosition_t; } navWaypointPosition_t;
typedef struct navDestinationPath_s { typedef struct navDestinationPath_s { // NOT USED
uint32_t distance; // meters * 100 uint32_t distance; // meters * 100
int32_t bearing; // deg * 100 int32_t bearing; // deg * 100
} navDestinationPath_t; } navDestinationPath_t;
@ -570,7 +571,7 @@ float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag); geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag);
/* Distance/bearing calculation */ /* Distance/bearing calculation */
bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos); bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos); // NOT USED
uint32_t distanceToFirstWP(void); uint32_t distanceToFirstWP(void);
/* Failsafe-forced RTH mode */ /* Failsafe-forced RTH mode */

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

@ -305,7 +305,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// velXY provides additional turn initiation distance based on an assumed 1 second delayed turn response time // 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 (posControl.wpDistance < (posControl.actualState.velXY + navLoiterRadius * turnStartFactor)) {
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) { 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); int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.bearing + 18000);
loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_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)); loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
@ -398,12 +398,12 @@ 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 waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
// courseVirtualCorrection initially used to determine current position relative to course line for later use // courseVirtualCorrection initially used to determine current position relative to course line for later use
int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing); int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
navCrossTrackError = 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 // 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) { if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) {
int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw); int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog);
// captureFactor adjusts distance/heading sensitivity balance when closing in on course line. // 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. // Closing distance threashold based on speed and an assumed 1 second response time.
@ -423,7 +423,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
// final courseVirtualCorrection value // final courseVirtualCorrection value
courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor; courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor;
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseVirtualCorrection); virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - courseVirtualCorrection);
} }
} }
@ -431,7 +431,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
* Calculate NAV heading error * Calculate NAV heading error
* Units are centidegrees * Units are centidegrees
*/ */
navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw); navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog);
// Forced turn direction // 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 // If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg
@ -461,7 +461,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0); const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0);
// Input error in (deg*100), output roll angle (deg*100) // Input error in (deg*100), output roll angle (deg*100)
float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.yaw + navHeadingError, posControl.actualState.yaw, US2S(deltaMicros), float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.cog + navHeadingError, posControl.actualState.cog, US2S(deltaMicros),
-DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), -DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
pidFlags); pidFlags);
@ -769,7 +769,7 @@ void calculateFixedWingInitialHoldPosition(fpVector3_t * pos)
void resetFixedWingHeadingController(void) void resetFixedWingHeadingController(void)
{ {
updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw)); updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.actualState.cog));
} }
void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs) void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)

8
src/main/navigation/navigation_pos_estimator.c
View file

@ -793,9 +793,9 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
static navigationTimer_t posPublishTimer; static navigationTimer_t posPublishTimer;
/* IMU operates in decidegrees while INAV operates in deg*100 /* IMU operates in decidegrees while INAV operates in deg*100
* Use course over ground for fixed wing navigation yaw/"heading" */ * Use course over ground when GPS heading valid */
int16_t yawValue = isGPSHeadingValid() && STATE(AIRPLANE) ? posEstimator.est.cog : attitude.values.yaw; int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw;
updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(yawValue), DECIDEGREES_TO_CENTIDEGREES(posEstimator.est.cog)); updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue));
/* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */ /* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) { if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
@ -835,7 +835,7 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
DEBUG_SET(DEBUG_POS_EST, 4, (int32_t) posEstimator.est.vel.y*1000.0F); // Speed estimate VY DEBUG_SET(DEBUG_POS_EST, 4, (int32_t) posEstimator.est.vel.y*1000.0F); // Speed estimate VY
DEBUG_SET(DEBUG_POS_EST, 6, (int32_t) attitude.values.yaw); // Yaw estimate (4 bytes still available here) DEBUG_SET(DEBUG_POS_EST, 6, (int32_t) attitude.values.yaw); // Yaw estimate (4 bytes still available here)
DEBUG_SET(DEBUG_POS_EST, 7, (int32_t) (posEstimator.flags & 0b1111111)<<20 | // navPositionEstimationFlags fit into 8bits DEBUG_SET(DEBUG_POS_EST, 7, (int32_t) (posEstimator.flags & 0b1111111)<<20 | // navPositionEstimationFlags fit into 8bits
(MIN(navEPH, 1000) & 0x3FF)<<10 | (MIN(navEPH, 1000) & 0x3FF)<<10 |
(MIN(navEPV, 1000) & 0x3FF)); // Horizontal and vertical uncertainties (max value = 1000, fit into 20bits) (MIN(navEPV, 1000) & 0x3FF)); // Horizontal and vertical uncertainties (max value = 1000, fit into 20bits)
} }
} }

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

@ -324,9 +324,9 @@ typedef struct {
typedef struct { typedef struct {
fpVector3_t targetPos; fpVector3_t targetPos;
int32_t yaw; int32_t course;
int32_t previousYaw; int32_t previousCourse;
timeMs_t lastYawAdjustmentTime; timeMs_t lastCourseAdjustmentTime;
} navCruise_t; } navCruise_t;
typedef struct { typedef struct {
@ -445,10 +445,10 @@ bool isMulticopterFlying(void);
navigationFSMStateFlags_t navGetCurrentStateFlags(void); navigationFSMStateFlags_t navGetCurrentStateFlags(void);
void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags); void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags);
void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask); void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
void setDesiredSurfaceOffset(float surfaceOffset); void setDesiredSurfaceOffset(float surfaceOffset);
void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); // NOT USED
void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode); void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode);
bool isNavHoldPositionActive(void); bool isNavHoldPositionActive(void);