mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 01:35:35 +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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -3855,7 +3850,7 @@ bool navigationPositionEstimateIsHealthy(void)
|
|||
navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
|
||||
{
|
||||
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)));
|
||||
|
||||
|
||||
if (usedBypass) {
|
||||
*usedBypass = false;
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue