mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-20 23:05:17 +03:00
New tracking accuracy method
This commit is contained in:
breadoven
parent
fd125bb017
commit
222ba4aea0
7 changed files with 99 additions and 77 deletions
|
|
@ -3274,21 +3274,21 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within
|
|||
|
||||
### nav_fw_wp_tracking_accuracy
|
||||
|
||||
Sets a reference distance to the waypoint course line that affects the accuracy of waypoint course tracking [m]. If set to 0 course tracking is disabled and the craft will head directly to the next waypoint from whatever position was achieved after the last waypoint turn. Otherwise course tracking will be enabled which will force the craft to quickly head toward and track along the the course line as closely as possible. The course tracking response can be tuned by changing the accuracy setting. Lower values will push the craft toward the course line quicker but may result in overshoot. 40 is a good starting point.
|
||||
Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Settings 1 to 5 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 2 is a good starting point. Set to 0 to disable.
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 0 | 0 | 100 |
|
||||
| 0 | 0 | 5 |
|
||||
|
||||
---
|
||||
|
||||
### nav_fw_wp_turn_smoothing
|
||||
|
||||
Smooths turns during WP missions by switching to a loiter path when within the nav_fw_loiter_radius from the waypoint. The loiter is centered relative to the waypoint such that the craft passes through the waypoint during the loiter at which point the waypoint is considered to have been reached.
|
||||
Smooths turns during WP missions by switching to a loiter turn at waypoints. 2 settings are possible. Setting 1 uses a loiter path that passes through the waypoint. Setting 2 uses a loiter path that cuts inside the turn without passing through the waypoint. Set to 0 to disable.
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| OFF | OFF | ON |
|
||||
| 0 | 0 | 2 |
|
||||
|
||||
---
|
||||
|
||||
|
|
|
|||
|
|
@ -127,7 +127,7 @@ int32_t wrap_18000(int32_t angle)
|
|||
|
||||
int32_t wrap_36000(int32_t angle)
|
||||
{
|
||||
if (angle > 36000)
|
||||
if (angle >= 36000)
|
||||
angle -= 36000;
|
||||
if (angle < 0)
|
||||
angle += 36000;
|
||||
|
|
|
|||
|
|
@ -2421,16 +2421,17 @@ groups:
|
|||
min: 0
|
||||
max: 9
|
||||
- name: nav_fw_wp_tracking_accuracy
|
||||
description: "Sets a reference distance to the waypoint course line that affects the accuracy of waypoint course tracking [m]. If set to 0 course tracking is disabled and the craft will head directly to the next waypoint from whatever position was achieved after the last waypoint turn. Otherwise course tracking will be enabled which will force the craft to quickly head toward and track along the the course line as closely as possible. The course tracking response can be tuned by changing the accuracy setting. Lower values will push the craft toward the course line quicker but may result in overshoot. 40 is a good starting point."
|
||||
description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Settings 1 to 5 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 2 is a good starting point. Set to 0 to disable."
|
||||
default_value: 0
|
||||
field: fw.waypoint_tracking_accuracy
|
||||
min: 0
|
||||
max: 100
|
||||
max: 5
|
||||
- name: nav_fw_wp_turn_smoothing
|
||||
description: "Smooths turns during WP missions by switching to a loiter path when within the nav_fw_loiter_radius from the waypoint. The loiter is centered relative to the waypoint such that the craft passes through the waypoint during the loiter at which point the waypoint is considered to have been reached."
|
||||
default_value: OFF
|
||||
description: "Smooths turns during WP missions by switching to a loiter turn at waypoints. 2 settings are possible. Setting 1 uses a loiter path that passes through the waypoint. Setting 2 uses a loiter path that cuts inside the turn without passing through the waypoint. Set to 0 to disable."
|
||||
default_value: 0
|
||||
field: fw.waypoint_turn_smoothing
|
||||
type: bool
|
||||
min: 0
|
||||
max: 2
|
||||
- name: nav_auto_speed
|
||||
description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]"
|
||||
default_value: 300
|
||||
|
|
|
|||
|
|
@ -209,8 +209,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
|
||||
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled
|
||||
.auto_disarm_delay = SETTING_NAV_FW_AUTO_DISARM_DELAY_DEFAULT, // ms - time delay to disarm when auto disarm after landing enabled
|
||||
.waypoint_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0 m, improves course tracking during FW WP missions
|
||||
.waypoint_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // OFF, smooths turns during FW WP mode missions
|
||||
.waypoint_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking during FW WP missions
|
||||
.waypoint_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // 0, smooths turns during FW WP mode missions
|
||||
}
|
||||
);
|
||||
|
||||
|
|
@ -2257,8 +2257,8 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w
|
|||
|
||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) {
|
||||
// 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 45 degrees
|
||||
uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 4500 : 10000;
|
||||
// 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) {
|
||||
return true;
|
||||
}
|
||||
|
|
@ -3327,12 +3327,12 @@ 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)
|
||||
if (isWaypointNavTrackingRoute()) {
|
||||
if (isWaypointNavTrackingActive() && !(posControl.activeWaypoint.pos.x == pos->x && posControl.activeWaypoint.pos.y == pos->y)) {
|
||||
posControl.activeWaypoint.yaw = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos);
|
||||
} else {
|
||||
posControl.activeWaypoint.yaw = calculateBearingToDestination(pos);
|
||||
}
|
||||
posControl.activeWaypoint.bearingToNextWp = -1; // reset to NO next WP (-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;
|
||||
|
||||
|
|
@ -3354,7 +3354,8 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
|
|||
if (navConfig()->fw.waypoint_turn_smoothing) {
|
||||
fpVector3_t posNextWp;
|
||||
if (getLocalPosNextWaypoint(&posNextWp)) {
|
||||
posControl.activeWaypoint.bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp);
|
||||
int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp);
|
||||
posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.yaw);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -3406,11 +3407,13 @@ float getActiveWaypointSpeed(void)
|
|||
}
|
||||
}
|
||||
|
||||
bool isWaypointNavTrackingRoute(void)
|
||||
bool isWaypointNavTrackingActive(void)
|
||||
{
|
||||
// true when established on route beyond first waypoint
|
||||
return (FLIGHT_MODE(NAV_WP_MODE) && posControl.activeWaypointIndex > 0) ||
|
||||
(posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex);
|
||||
// NAV_WP_MODE flag used rather than state flag NAV_AUTO_WP to ensure heading to initial waypoint
|
||||
// is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP.
|
||||
// (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called)
|
||||
|
||||
return FLIGHT_MODE(NAV_WP_MODE) || (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex);
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -307,8 +307,8 @@ typedef struct navConfig_s {
|
|||
uint8_t yawControlDeadband;
|
||||
uint8_t soaring_pitch_deadband; // soaring mode pitch angle deadband (deg)
|
||||
uint16_t auto_disarm_delay; // fixed wing disarm delay for landing detector
|
||||
uint8_t waypoint_tracking_accuracy; // fixed wing tracking distance accuracy [m]
|
||||
uint8_t waypoint_turn_smoothing; // Smooths turns during WP missions
|
||||
uint8_t waypoint_tracking_accuracy; // fixed wing tracking accuracy response factor
|
||||
uint8_t waypoint_turn_smoothing; // WP mission turn smoothing options
|
||||
} fw;
|
||||
} navConfig_t;
|
||||
|
||||
|
|
@ -376,7 +376,7 @@ extern radar_pois_t radar_pois[RADAR_MAX_POIS];
|
|||
typedef struct {
|
||||
fpVector3_t pos;
|
||||
int32_t yaw; // centidegrees
|
||||
int32_t bearingToNextWp; // centidegrees
|
||||
int32_t nextTurnAngle; // centidegrees
|
||||
} navWaypointPosition_t;
|
||||
|
||||
typedef struct navDestinationPath_s {
|
||||
|
|
|
|||
|
|
@ -73,7 +73,6 @@ static float throttleSpeedAdjustment = 0;
|
|||
static bool isAutoThrottleManuallyIncreased = false;
|
||||
static int32_t navHeadingError;
|
||||
static int8_t loiterDirYaw = 1;
|
||||
static bool needToCalculateCircularLoiter = false;
|
||||
|
||||
// Calculates the cutoff frequency for smoothing out roll/pitch commands
|
||||
// control_smoothness valid range from 0 to 9
|
||||
|
|
@ -276,34 +275,47 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
|||
|
||||
uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius);
|
||||
fpVector3_t loiterCenterPos = posControl.desiredState.pos;
|
||||
int8_t turnDirection = loiterDirection();
|
||||
int8_t loiterTurnDirection = loiterDirection();
|
||||
bool isWpTrackingActive = isWaypointNavTrackingActive();
|
||||
|
||||
// Detemine if a circular loiter is required.
|
||||
// For waypoints only use circular loiter when angular visibility is > 30 degs, otherwise head straight toward target
|
||||
#define TAN_15DEG 0.26795f
|
||||
needToCalculateCircularLoiter = isNavHoldPositionActive() &&
|
||||
bool needToCalculateCircularLoiter = isNavHoldPositionActive() &&
|
||||
(distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
|
||||
(distanceToActualTarget > 50.0f);
|
||||
|
||||
/* WP turn smoothing option - switch to loiter path when distance to waypoint < navLoiterRadius.
|
||||
* Loiter centered on point inside turn at navLoiterRadius distance from waypoint and
|
||||
* on a bearing midway between current and next waypoint course bearings
|
||||
* Only for turns > 30 degs, navLoiterRadius factored down between 30 to 60 degs to align with course line */
|
||||
/* WP turn smoothing option - 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 */
|
||||
|
||||
int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle);
|
||||
posControl.flags.wpTurnSmoothingActive = false;
|
||||
if (navConfig()->fw.waypoint_turn_smoothing && isWaypointNavTrackingRoute() && !needToCalculateCircularLoiter &&
|
||||
posControl.activeWaypoint.bearingToNextWp != -1) {
|
||||
int32_t turnAngle = wrap_18000(posControl.activeWaypoint.bearingToNextWp - posControl.activeWaypoint.yaw);
|
||||
float turnFactor = ABS(turnAngle) < 3000 ? 0.0f : constrainf(ABS(turnAngle) / 6000.0f, 0.5f, 1.0f);
|
||||
if (waypointTurnAngle > 3000 && waypointTurnAngle < 12000 && isWpTrackingActive && !needToCalculateCircularLoiter) {
|
||||
// turnFactor adjusts start of loiter based on turn angle
|
||||
float turnFactor = 0.0f;
|
||||
if (navConfig()->fw.waypoint_turn_smoothing == 1) { // passes through WP
|
||||
turnFactor = waypointTurnAngle / 6000.0f;
|
||||
} else {
|
||||
turnFactor = 1.0f / tan_approx(CENTIDEGREES_TO_RADIANS(9000.0f - waypointTurnAngle / 2.0f)); // cut inside turn missing WP
|
||||
}
|
||||
constrainf(turnFactor, 0.5f, 1.5f);
|
||||
if (posControl.wpDistance < navLoiterRadius * turnFactor) {
|
||||
int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.bearingToNextWp - posControl.activeWaypoint.yaw - 18000)) / 2) + posControl.activeWaypoint.yaw + 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));
|
||||
int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.yaw + 18000);
|
||||
float distToTurnCentre = navLoiterRadius;
|
||||
if (navConfig()->fw.waypoint_turn_smoothing == 2) {
|
||||
distToTurnCentre = navLoiterRadius / sin_approx(CENTIDEGREES_TO_RADIANS(9000.0f - 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));
|
||||
|
||||
posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
|
||||
|
||||
// turn direction to next waypoint
|
||||
turnDirection = turnAngle > 0 ? 1 : -1; // 1 = right
|
||||
loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right
|
||||
|
||||
needToCalculateCircularLoiter = posControl.flags.wpTurnSmoothingActive = true;
|
||||
}
|
||||
|
|
@ -311,7 +323,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
|||
|
||||
// We are closing in on a waypoint, calculate circular loiter if required
|
||||
if (needToCalculateCircularLoiter) {
|
||||
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(turnDirection * 45.0f);
|
||||
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterTurnDirection * 45.0f);
|
||||
float loiterTargetX = loiterCenterPos.x + navLoiterRadius * cos_approx(loiterAngle);
|
||||
float loiterTargetY = loiterCenterPos.y + navLoiterRadius * sin_approx(loiterAngle);
|
||||
|
||||
|
|
@ -319,6 +331,43 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
|||
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
posErrorY = loiterTargetY - navGetCurrentActualPositionAndVelocity()->pos.y;
|
||||
distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY);
|
||||
} else if (navConfig()->fw.waypoint_tracking_accuracy && isWpTrackingActive) {
|
||||
// track along waypoint course line if tracking accuracy used
|
||||
fpVector3_t tempPos;
|
||||
fpVector3_t currentWPPos = posControl.activeWaypoint.pos;
|
||||
uint32_t currentCourse = posControl.activeWaypoint.yaw;
|
||||
uint8_t distanceFactor;
|
||||
uint8_t setting = navConfig()->fw.waypoint_tracking_accuracy;
|
||||
|
||||
if (currentCourse == 9000 || currentCourse == 27000) {
|
||||
distanceFactor = constrain(4 + setting - (fabsf(posErrorX) / 750), setting, setting + 4);
|
||||
tempPos.x = currentWPPos.x;
|
||||
tempPos.y = navGetCurrentActualPositionAndVelocity()->pos.y + trackingDistance * distanceFactor * (currentCourse == 9000 ? 1 : -1);
|
||||
} else if (currentCourse == 0 || currentCourse == 18000) {
|
||||
distanceFactor = constrain(4 + setting - (fabsf(posErrorY) / 750), setting, setting + 4);
|
||||
tempPos.x = navGetCurrentActualPositionAndVelocity()->pos.x + trackingDistance * distanceFactor * (currentCourse == 0 ? 1 : -1);
|
||||
tempPos.y = currentWPPos.y;
|
||||
} else {
|
||||
// using y = mx + c
|
||||
// constant2 = y axis intercept of line normal to course line passing through actual position
|
||||
// used to calculate position on course line normal to actual position
|
||||
float gradient = tan_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.yaw));
|
||||
float constant = currentWPPos.y - gradient * currentWPPos.x;
|
||||
float constant2 = navGetCurrentActualPositionAndVelocity()->pos.y + navGetCurrentActualPositionAndVelocity()->pos.x / gradient;
|
||||
tempPos.x = (constant2 - constant) / (gradient + 1 / gradient);
|
||||
tempPos.y = gradient * tempPos.x + constant;
|
||||
distanceFactor = constrain(4 + setting - (calculateDistanceToDestination(&tempPos) / 750), setting, setting + 4);
|
||||
if (fabsf(gradient) > 1) { // increment in y
|
||||
tempPos.y += trackingDistance * distanceFactor * (currentCourse < 18000 ? 1 : -1);
|
||||
tempPos.x = (tempPos.y - constant) / gradient;
|
||||
} else { // increment in x
|
||||
tempPos.x += trackingDistance * distanceFactor * (currentCourse > 27000 || currentCourse < 9000 ? 1 : -1);
|
||||
tempPos.y = gradient * tempPos.x + constant;
|
||||
}
|
||||
}
|
||||
posErrorX = tempPos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
posErrorY = tempPos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
|
||||
distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY);
|
||||
}
|
||||
|
||||
// Calculate virtual waypoint
|
||||
|
|
@ -383,37 +432,6 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
|
|||
// We have virtual position target, calculate heading error
|
||||
int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
|
||||
|
||||
/* If waypoint tracking enabled quickly force craft toward waypoint course line and track along it */
|
||||
if (navConfig()->fw.waypoint_tracking_accuracy && isWaypointNavTrackingRoute() && !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) {
|
||||
fpVector3_t virtualCoursePoint;
|
||||
virtualCoursePoint.x = posControl.activeWaypoint.pos.x -
|
||||
posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.yaw));
|
||||
virtualCoursePoint.y = posControl.activeWaypoint.pos.y -
|
||||
posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.yaw));
|
||||
float distToCourseLine = calculateDistanceToDestination(&virtualCoursePoint);
|
||||
|
||||
// courseVirtualCorrection initially used to determine current position relative to course line for later use
|
||||
int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing);
|
||||
|
||||
// bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy
|
||||
float courseCorrectionFactor = constrainf(distToCourseLine / (100.0f * navConfig()->fw.waypoint_tracking_accuracy), 0.0f, 1.0f);
|
||||
courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
|
||||
|
||||
int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw);
|
||||
float courseHeadingFactor = constrainf(sq(courseHeadingError / 18000.0f), 0.0f, 1.0f);
|
||||
courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor;
|
||||
|
||||
courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f);
|
||||
|
||||
// final courseVirtualCorrection using max 80 deg heading adjustment toward course line
|
||||
courseVirtualCorrection = 8000 * courseCorrectionFactor;
|
||||
|
||||
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseVirtualCorrection);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Calculate NAV heading error
|
||||
* Units are centidegrees
|
||||
|
|
|
|||
|
|
@ -398,7 +398,7 @@ typedef struct {
|
|||
int8_t loadedMultiMissionStartWP; // selected multi mission start WP
|
||||
int8_t loadedMultiMissionWPCount; // number of WPs in selected multi mission
|
||||
#endif
|
||||
navWaypointPosition_t activeWaypoint; // Local position, current bearing and bearing to next WP, filled on waypoint activation
|
||||
navWaypointPosition_t activeWaypoint; // Local position, current bearing and turn angle to next WP, filled on waypoint activation
|
||||
int8_t activeWaypointIndex;
|
||||
float wpInitialAltitude; // Altitude at start of WP
|
||||
float wpInitialDistance; // Distance when starting flight to WP
|
||||
|
|
@ -455,7 +455,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
|
|||
bool isNavHoldPositionActive(void);
|
||||
bool isLastMissionWaypoint(void);
|
||||
float getActiveWaypointSpeed(void);
|
||||
bool isWaypointNavTrackingRoute(void);
|
||||
bool isWaypointNavTrackingActive(void);
|
||||
|
||||
void updateActualHeading(bool headingValid, int32_t newHeading);
|
||||
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue