mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-19 06:15:14 +03:00
first cut
This commit is contained in:
breadoven
parent
5d84300545
commit
a366ae4671
5 changed files with 69 additions and 73 deletions
|
|
@ -256,9 +256,9 @@ static void clearJumpCounters(void);
|
|||
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 yaw, int32_t distance);
|
||||
void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance);
|
||||
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointYaw);
|
||||
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);
|
||||
static navigationFSMEvent_t nextForNonGeoStates(void);
|
||||
|
|
@ -1072,9 +1072,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
|
|||
resetPositionController();
|
||||
}
|
||||
|
||||
posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow
|
||||
posControl.cruise.previousYaw = posControl.cruise.yaw;
|
||||
posControl.cruise.lastYawAdjustmentTime = 0;
|
||||
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
|
||||
}
|
||||
|
|
@ -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
|
||||
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.
|
||||
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;
|
||||
posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
|
||||
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
|
||||
posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
|
||||
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.lastYawAdjustmentTime > 4000)
|
||||
posControl.cruise.previousYaw = posControl.cruise.yaw;
|
||||
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.yaw, distance);
|
||||
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.yaw, distance);
|
||||
calculateNewCruiseTarget(&posControl.cruise.targetPos, posControl.cruise.course, distance);
|
||||
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;
|
||||
}
|
||||
|
|
@ -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
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
|
||||
posControl.cruise.lastYawAdjustmentTime = millis();
|
||||
posControl.cruise.course = posControl.actualState.cog; //store current course
|
||||
posControl.cruise.lastCourseAdjustmentTime = millis();
|
||||
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
|
||||
calculateInitialHoldPosition(&targetHoldPos);
|
||||
} else {
|
||||
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away Linear climb
|
||||
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.cog, 100000.0f); // 1km away Linear climb
|
||||
}
|
||||
} else {
|
||||
// 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
|
||||
}
|
||||
|
||||
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.yaw)) {
|
||||
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
|
||||
posControl.activeRthTBPointIndex--;
|
||||
|
||||
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)) {
|
||||
// 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
|
||||
} else {
|
||||
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
|
||||
// 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
|
||||
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
|
||||
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
|
||||
} else {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
@ -1629,7 +1629,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
|
|||
case NAV_WP_ACTION_HOLD_TIME:
|
||||
case NAV_WP_ACTION_WAYPOINT:
|
||||
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
|
||||
}
|
||||
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
|
||||
// to the home point.
|
||||
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;
|
||||
}
|
||||
/* 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.cog = newGroundCourse; // currently only used for OSD display
|
||||
posControl.actualState.cog = newGroundCourse;
|
||||
posControl.flags.estHeadingStatus = newEstHeading;
|
||||
|
||||
/* Precompute sin/cos of yaw angle */
|
||||
|
|
@ -2207,7 +2202,7 @@ int32_t calculateBearingBetweenLocalPositions(const fpVector3_t * startPos, cons
|
|||
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 ||
|
||||
posControl.flags.estHeadingStatus == EST_NONE) {
|
||||
|
|
@ -2254,9 +2249,9 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
|
|||
|
||||
/*-----------------------------------------------------------
|
||||
* 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);
|
||||
|
||||
|
|
@ -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
|
||||
// Same method for turn smoothing option but relative bearing set at 60 degrees
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
@ -2400,7 +2395,7 @@ bool validateRTHSanityChecker(void)
|
|||
/*-----------------------------------------------------------
|
||||
* 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
|
||||
if ((useMask & NAV_POS_UPDATE_XY) != 0) {
|
||||
|
|
@ -2426,7 +2421,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t
|
|||
// Heading
|
||||
if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
|
||||
// Heading
|
||||
posControl.rthState.homePosition.yaw = yaw;
|
||||
posControl.rthState.homePosition.heading = heading;
|
||||
if (homeFlags & NAV_HOME_VALID_HEADING) {
|
||||
posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
|
||||
} else {
|
||||
|
|
@ -2677,7 +2672,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
|
|||
if (posControl.activeRthTBPointIndex < 0) {
|
||||
saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(50);
|
||||
|
||||
previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw);
|
||||
previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.cog);
|
||||
previousTBTripDist = posControl.totalTripDistance;
|
||||
} else {
|
||||
// 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->y = navGetCurrentActualPositionAndVelocity()->pos.y + distance * sin_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(bearing));
|
||||
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->y = origin->y + distance * sin_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(course));
|
||||
origin->z = origin->z;
|
||||
}
|
||||
|
||||
|
|
@ -3388,18 +3383,18 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
|
|||
|
||||
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)) {
|
||||
posControl.activeWaypoint.yaw = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos);
|
||||
posControl.activeWaypoint.bearing = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos);
|
||||
} 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.pos = *pos;
|
||||
|
||||
// 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)
|
||||
|
|
@ -3417,7 +3412,7 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
|
|||
fpVector3_t posNextWp;
|
||||
if (getLocalPosNextWaypoint(&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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -4340,7 +4335,7 @@ bool abortLaunchAllowed(void)
|
|||
|
||||
int32_t navigationGetHomeHeading(void)
|
||||
{
|
||||
return posControl.rthState.homePosition.yaw;
|
||||
return posControl.rthState.homePosition.heading;
|
||||
}
|
||||
|
||||
// returns m/s
|
||||
|
|
@ -4363,5 +4358,5 @@ bool isAdjustingHeading(void) {
|
|||
}
|
||||
|
||||
int32_t getCruiseHeadingAdjustment(void) {
|
||||
return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw);
|
||||
return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -395,11 +395,12 @@ extern radar_pois_t radar_pois[RADAR_MAX_POIS];
|
|||
|
||||
typedef struct {
|
||||
fpVector3_t pos;
|
||||
int32_t yaw; // centidegrees
|
||||
int32_t heading; // centidegrees
|
||||
int32_t bearing; // centidegrees
|
||||
int32_t nextTurnAngle; // centidegrees
|
||||
} navWaypointPosition_t;
|
||||
|
||||
typedef struct navDestinationPath_s {
|
||||
typedef struct navDestinationPath_s { // NOT USED
|
||||
uint32_t distance; // meters * 100
|
||||
int32_t bearing; // deg * 100
|
||||
} navDestinationPath_t;
|
||||
|
|
@ -570,7 +571,7 @@ float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
|
|||
geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag);
|
||||
|
||||
/* 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);
|
||||
|
||||
/* Failsafe-forced RTH mode */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
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);
|
||||
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.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 (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
|
||||
// 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)));
|
||||
|
||||
// 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);
|
||||
if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) {
|
||||
int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog);
|
||||
|
||||
// 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.
|
||||
|
|
@ -423,7 +423,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
|
|||
|
||||
// final courseVirtualCorrection value
|
||||
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
|
||||
* Units are centidegrees
|
||||
*/
|
||||
navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw);
|
||||
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
|
||||
|
|
@ -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);
|
||||
|
||||
// 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),
|
||||
pidFlags);
|
||||
|
|
@ -769,7 +769,7 @@ void calculateFixedWingInitialHoldPosition(fpVector3_t * pos)
|
|||
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -793,9 +793,9 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
|
|||
static navigationTimer_t posPublishTimer;
|
||||
|
||||
/* IMU operates in decidegrees while INAV operates in deg*100
|
||||
* Use course over ground for fixed wing navigation yaw/"heading" */
|
||||
int16_t yawValue = isGPSHeadingValid() && STATE(AIRPLANE) ? posEstimator.est.cog : attitude.values.yaw;
|
||||
updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(yawValue), DECIDEGREES_TO_CENTIDEGREES(posEstimator.est.cog));
|
||||
* Use course over ground when GPS heading valid */
|
||||
int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw;
|
||||
updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue));
|
||||
|
||||
/* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
|
||||
if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
|
||||
|
|
|
|||
|
|
@ -324,9 +324,9 @@ typedef struct {
|
|||
|
||||
typedef struct {
|
||||
fpVector3_t targetPos;
|
||||
int32_t yaw;
|
||||
int32_t previousYaw;
|
||||
timeMs_t lastYawAdjustmentTime;
|
||||
int32_t course;
|
||||
int32_t previousCourse;
|
||||
timeMs_t lastCourseAdjustmentTime;
|
||||
} navCruise_t;
|
||||
|
||||
typedef struct {
|
||||
|
|
@ -445,10 +445,10 @@ bool isMulticopterFlying(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 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);
|
||||
|
||||
bool isNavHoldPositionActive(void);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue