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

...

Browse source
This commit is contained in:
Scavanger 2024-02-10 13:50:06 -03:00
parent f42df69e0b
commit 5a5ee09034
3 changed files with 20 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -291,6 +291,11 @@ bool validateRTHSanityChecker(void);
void updateHomePosition(void); void updateHomePosition(void);
bool abortLaunchAllowed(void); bool abortLaunchAllowed(void);
static bool rthAltControlStickOverrideCheck(unsigned axis);
static void updateRthTrackback(bool forceSaveTrackPoint);
static fpVector3_t * rthGetTrackbackPos(void);
static int32_t calcWindDiff(int32_t heading, int32_t windHeading); static int32_t calcWindDiff(int32_t heading, int32_t windHeading);
static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle); static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle);
static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos); static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos);
@ -2291,7 +2296,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
tmpPos.z = finalApproachAlt; tmpPos.z = finalApproachAlt;
posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos; posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos;
calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH], dir, navConfig()->fw.loiter_radius * 2.5); calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH], dir, MAX((uint32_t)navConfig()->fw.loiter_radius * 4, navFwAutolandConfig()->approachLength / 2));
tmpPos.z = posControl.fwLandState.landAproachAltAgl; tmpPos.z = posControl.fwLandState.landAproachAltAgl;
posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_TURN] = tmpPos; posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_TURN] = tmpPos;
@ -4148,7 +4153,10 @@ void applyWaypointNavigationAndAltitudeHold(void)
// ensure WP missions always restart from first waypoint after disarm // ensure WP missions always restart from first waypoint after disarm
posControl.activeWaypointIndex = posControl.startWpIndex; posControl.activeWaypointIndex = posControl.startWpIndex;
// Reset RTH trackback // Reset RTH trackback
resetRthTrackBack(); posControl.activeRthTBPointIndex = -1;
posControl.flags.rthTrackbackActive = false;
posControl.rthTBWrapAroundCounter = -1;
return; return;
} }

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

@ -465,6 +465,12 @@ typedef struct {
timeMs_t wpReachedTime; // Time the waypoint was reached timeMs_t wpReachedTime; // Time the waypoint was reached
bool wpAltitudeReached; // WP altitude achieved bool wpAltitudeReached; // WP altitude achieved
/* RTH Trackback */
fpVector3_t rthTBPointsList[NAV_RTH_TRACKBACK_POINTS];
int8_t rthTBLastSavedIndex; // last trackback point index saved
int8_t activeRthTBPointIndex;
int8_t rthTBWrapAroundCounter; // stores trackpoint array overwrite index position
/* Fixedwing autoland */ /* Fixedwing autoland */
fwLandState_t fwLandState; fwLandState_t fwLandState;

6
src/main/target/SITL/sim/realFlight.c
View file

@ -59,8 +59,10 @@
#define RF_PORT 18083 #define RF_PORT 18083
#define RF_MAX_CHANNEL_COUNT 12 #define RF_MAX_CHANNEL_COUNT 12
// RealFlight scenerys doesn't represent real landscapes, so fake some nice coords // RealFlight scenerys doesn't represent real landscapes, so fake some nice coords
#define FAKE_LAT 37.277127f //#define FAKE_LAT 37.277127f
#define FAKE_LON -115.799669f //#define FAKE_LON -115.799669f
#define FAKE_LAT 0
#define FAKE_LON 0
static uint8_t pwmMapping[RF_MAX_PWM_OUTS]; static uint8_t pwmMapping[RF_MAX_PWM_OUTS];