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

first build

Browse source
This commit is contained in:
breadoven 2022-07-02 10:28:15 +01:00
parent 99681da2c6
commit aa6e77df64
7 changed files with 151 additions and 36 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -100,7 +100,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 1);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 1);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -209,6 +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 cm
.wp_turn_smoothing_dist = SETTING_NAV_FW_WP_TURN_SMOOTHING_DIST_DEFAULT, // 0 cm
}
);
@ -249,7 +251,7 @@ 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 isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome);
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointYaw);
bool isWaypointAltitudeReached(void);
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
static navigationFSMEvent_t nextForNonGeoStates(void);
@ -1332,7 +1334,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, false) || isWaypointMissed(&posControl.activeWaypoint)) {
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.yaw)) {
posControl.activeRthTBPointIndex--;
if (posControl.rthTBWrapAroundCounter > -1 && posControl.activeRthTBPointIndex < 0) {
@ -1366,7 +1368,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
if (isWaypointPositionReached(tmpHomePos, true)) {
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);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
@ -1622,7 +1624,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, false) || isWaypointMissed(&posControl.activeWaypoint)) {
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.yaw)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
}
else {
@ -2186,6 +2188,14 @@ int32_t calculateBearingToDestination(const fpVector3_t * destinationPos)
return calculateBearingFromDelta(deltaX, deltaY);
}
int32_t calculateBearingBetweenLocalPositions(const fpVector3_t * startPos, const fpVector3_t * endPos)
{
const float deltaX = endPos->x - startPos->x;
const float deltaY = endPos->y - startPos->y;
return calculateBearingFromDelta(deltaX, deltaY);
}
bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos)
{
if (posControl.flags.estPosStatus == EST_NONE ||
@ -2203,35 +2213,67 @@ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3
return true;
}
static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
{
// Only for WP Mode not Trackback. Ignore non geo waypoints except RTH and JUMP.
if (FLIGHT_MODE(NAV_WP_MODE) && !isLastMissionWaypoint()) {
navWaypointActions_e nextWpAction = posControl.waypointList[posControl.activeWaypointIndex + 1].action;
if (!(nextWpAction == NAV_WP_ACTION_SET_POI || nextWpAction == NAV_WP_ACTION_SET_HEAD)) {
uint8_t nextWpIndex = posControl.activeWaypointIndex + 1;
if (nextWpAction == NAV_WP_ACTION_JUMP) {
if (posControl.waypointList[posControl.activeWaypointIndex + 1].p3 != 0 ||
posControl.waypointList[posControl.activeWaypointIndex + 1].p2 == -1) {
nextWpIndex = posControl.waypointList[posControl.activeWaypointIndex + 1].p1;
} else if (posControl.activeWaypointIndex + 2 <= posControl.waypointCount - 1) {
if (posControl.waypointList[posControl.activeWaypointIndex + 2].action != NAV_WP_ACTION_JUMP) {
nextWpIndex++;
} else {
return false; // give up - too complicated
}
}
}
mapWaypointToLocalPosition(nextWpPos, &posControl.waypointList[nextWpIndex], 0);
return true;
}
}
return false; // no position available
}
/*-----------------------------------------------------------
* Check if waypoint is/was reached. Assume that waypoint-yaw stores initial bearing
* Check if waypoint is/was reached.
* waypointYaw stores initial bearing to waypoint
*-----------------------------------------------------------*/
bool isWaypointMissed(const navWaypointPosition_t * waypoint)
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointYaw)
{
int32_t bearingError = calculateBearingToDestination(&waypoint->pos) - waypoint->yaw;
bearingError = wrap_18000(bearingError);
posControl.wpDistance = calculateDistanceToDestination(waypointPos);
return ABS(bearingError) > 10000; // TRUE if we passed the waypoint by 100 degrees
}
static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome)
{
// We consider waypoint reached if within specified radius
posControl.wpDistance = calculateDistanceToDestination(pos);
if (STATE(FIXED_WING_LEGACY) && isWaypointHome) {
// Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check
return (posControl.wpDistance <= navConfig()->general.waypoint_radius)
|| (posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius
// Airplane will do a circular loiter at hold waypoints and might never approach them closer than waypoint_radius
// Check within 10% margin of circular loiter radius
if (STATE(AIRPLANE) && isNavHoldPositionActive() && posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)) {
return true;
}
else {
return (posControl.wpDistance <= navConfig()->general.waypoint_radius);
}
}
bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome)
{
return isWaypointPositionReached(&waypoint->pos, isWaypointHome);
uint16_t turnEarlyDistance = 0;
if (FLIGHT_MODE(NAV_WP_MODE)) {
// Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypointYaw
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > 10000) {
return true;
}
// fixed wing option to reach waypoint earlier to help smooth tighter turns to next waypoint.
// factor applied from 1 to 10 for turns > 60 to 105 degs and over
if (navConfig()->fw.wp_turn_smoothing_dist && STATE(AIRPLANE) && posControl.activeWaypointIndex > 0) {
fpVector3_t nextWpPos;
if (getLocalPosNextWaypoint(&nextWpPos)) {
int32_t bearingToNextWP = ABS(wrap_18000(calculateBearingBetweenLocalPositions(waypointPos, &nextWpPos) - *waypointYaw));
turnEarlyDistance = navConfig()->fw.wp_turn_smoothing_dist * constrain((bearingToNextWP - 5500) / 500, 0, 10);
}
}
}
return posControl.wpDistance <= (navConfig()->general.waypoint_radius + turnEarlyDistance);
}
bool isWaypointAltitudeReached(void)
@ -3293,10 +3335,14 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
{
posControl.activeWaypoint.pos = *pos;
// Calculate bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints)
if (isWaypointNavTrackingRoute()) {
posControl.activeWaypoint.yaw = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos);
} else {
posControl.activeWaypoint.yaw = calculateBearingToDestination(pos);
}
// Calculate initial bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints)
posControl.activeWaypoint.yaw = calculateBearingToDestination(pos);
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);
@ -3361,6 +3407,13 @@ float getActiveWaypointSpeed(void)
}
}
bool isWaypointNavTrackingRoute(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);
}
/*-----------------------------------------------------------
* Process adjustments to alt, pos and yaw controllers
*-----------------------------------------------------------*/