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

remove redundant function + update

Browse source
This commit is contained in:
breadoven 2022-12-19 00:16:16 +00:00
parent 1015e19c81
commit e7f9f6e135
2 changed files with 7 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -255,7 +255,6 @@ 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 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); static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointYaw);
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);
@ -2798,13 +2797,6 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t dista
farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z; farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
} }
void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, 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->z = origin->z;
}
/*----------------------------------------------------------- /*-----------------------------------------------------------
* NAV land detector * NAV land detector
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/

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

@ -266,6 +266,10 @@ static int8_t loiterDirection(void) {
static void calculateVirtualPositionTarget_FW(float trackingPeriod) static void calculateVirtualPositionTarget_FW(float trackingPeriod)
{ {
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
return;
}
float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x; float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y; float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
@ -391,11 +395,11 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
static bool errorIsDecreasing; static bool errorIsDecreasing;
static bool forceTurnDirection = false; static bool forceTurnDirection = false;
// We have virtual position target, calculate heading error
int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
virtualTargetBearing = posControl.desiredState.yaw; virtualTargetBearing = posControl.desiredState.yaw;
} else {
// 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 closely track along it */ /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */