mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 01:35:35 +03:00
Merge branch 'master' into MrD_Add-odometer-to-OSD
This commit is contained in:
Darren Lines
commit
de2c439b5d
272 changed files with 8622 additions and 2937 deletions
|
|
@ -62,6 +62,8 @@
|
|||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#include "programming/global_variables.h"
|
||||
|
||||
// Multirotors:
|
||||
#define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt)
|
||||
#define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend)
|
||||
|
|
@ -99,13 +101,12 @@ 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, 2);
|
||||
#endif
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 3);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4);
|
||||
|
||||
PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
||||
.general = {
|
||||
|
||||
.flags = {
|
||||
.use_thr_mid_for_althold = SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD_DEFAULT,
|
||||
.extra_arming_safety = SETTING_NAV_EXTRA_ARMING_SAFETY_DEFAULT,
|
||||
.user_control_mode = SETTING_NAV_USER_CONTROL_MODE_DEFAULT,
|
||||
.rth_alt_control_mode = SETTING_NAV_RTH_ALT_MODE_DEFAULT,
|
||||
|
|
@ -118,10 +119,12 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.rth_alt_control_override = SETTING_NAV_RTH_ALT_CONTROL_OVERRIDE_DEFAULT, // Override RTH Altitude and Climb First using Pitch and Roll stick
|
||||
.nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT,
|
||||
.safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT,
|
||||
.mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT, // Allow mode switch toggle to reset Mission Planner WPs
|
||||
.waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action
|
||||
.soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled
|
||||
.rth_trackback_mode = SETTING_NAV_RTH_TRACKBACK_MODE_DEFAULT // RTH trackback useage mode
|
||||
.mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT, // Allow mode switch toggle to reset Mission Planner WPs
|
||||
.waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action
|
||||
.soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled
|
||||
.rth_trackback_mode = SETTING_NAV_RTH_TRACKBACK_MODE_DEFAULT, // RTH trackback useage mode
|
||||
.rth_use_linear_descent = SETTING_NAV_RTH_USE_LINEAR_DESCENT_DEFAULT, // Use linear descent during RTH
|
||||
.landing_bump_detection = SETTING_NAV_LANDING_BUMP_DETECTION_DEFAULT, // Detect landing based on touchdown G bump
|
||||
},
|
||||
|
||||
// General navigation parameters
|
||||
|
|
@ -154,6 +157,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.waypoint_enforce_altitude = SETTING_NAV_WP_ENFORCE_ALTITUDE_DEFAULT, // Forces set wp altitude to be achieved
|
||||
.land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT, // Changes sensitivity of landing detection
|
||||
.auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled
|
||||
.rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
|
||||
},
|
||||
|
||||
// MC-specific
|
||||
|
|
@ -171,9 +175,10 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle
|
||||
#endif
|
||||
|
||||
.posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100
|
||||
.posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT, // posResponseExpo * 100
|
||||
.posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100
|
||||
.posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT, // posResponseExpo * 100
|
||||
.slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT,
|
||||
.althold_throttle_type = SETTING_NAV_MC_ALTHOLD_THROTTLE_DEFAULT, // STICK
|
||||
},
|
||||
|
||||
// Fixed wing
|
||||
|
|
@ -230,10 +235,10 @@ EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
|
|||
int16_t navCurrentState;
|
||||
int16_t navActualVelocity[3];
|
||||
int16_t navDesiredVelocity[3];
|
||||
int16_t navActualHeading;
|
||||
int16_t navDesiredHeading;
|
||||
int32_t navTargetPosition[3];
|
||||
int32_t navLatestActualPosition[3];
|
||||
int16_t navActualHeading;
|
||||
uint16_t navDesiredHeading;
|
||||
int16_t navActualSurface;
|
||||
uint16_t navFlags;
|
||||
uint16_t navEPH;
|
||||
|
|
@ -482,7 +487,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
|
|
@ -984,12 +989,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navi
|
|||
|
||||
resetGCSFlags();
|
||||
|
||||
// If surface tracking mode changed value - reset altitude controller
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
|
||||
// Prepare altitude controller if idle, RTH or WP modes active or surface mode status changed
|
||||
if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP) || terrainFollowingToggled) {
|
||||
resetAltitudeController(navTerrainFollowingRequested());
|
||||
}
|
||||
|
||||
if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0) || terrainFollowingToggled) {
|
||||
setupAltitudeController();
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
|
||||
}
|
||||
|
|
@ -1017,20 +1019,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
|
|||
|
||||
resetGCSFlags();
|
||||
|
||||
if ((prevFlags & NAV_CTL_POS) == 0) {
|
||||
resetPositionController();
|
||||
}
|
||||
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
|
||||
// Prepare altitude controller if idle, RTH or WP modes active or surface mode status changed
|
||||
if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP) || terrainFollowingToggled) {
|
||||
resetAltitudeController(navTerrainFollowingRequested());
|
||||
setupAltitudeController();
|
||||
}
|
||||
|
||||
if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0) || terrainFollowingToggled) {
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
|
||||
}
|
||||
|
||||
if ((previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING) && (previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME) && (previousState != NAV_STATE_RTH_LANDING)) {
|
||||
// Prepare position controller if idle or current Mode NOT active in position hold state
|
||||
if (previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME &&
|
||||
previousState != NAV_STATE_RTH_LANDING && previousState != NAV_STATE_WAYPOINT_RTH_LAND &&
|
||||
previousState != NAV_STATE_WAYPOINT_FINISHED && previousState != NAV_STATE_WAYPOINT_HOLD_TIME)
|
||||
{
|
||||
resetPositionController();
|
||||
|
||||
fpVector3_t targetHoldPos;
|
||||
calculateInitialHoldPosition(&targetHoldPos);
|
||||
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
|
|
@ -1054,18 +1056,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
|
|||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
/////////////////
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
|
||||
if (!STATE(FIXED_WING_LEGACY)) { // Only on FW for now
|
||||
return NAV_FSM_EVENT_ERROR;
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_CRUISE, 0, 1);
|
||||
// Switch to IDLE if we do not have an healty position. Try the next iteration.
|
||||
if (checkForPositionSensorTimeout()) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
} // Switch to IDLE if we do not have an healty position. Try the next iteration.
|
||||
}
|
||||
|
||||
resetPositionController();
|
||||
|
||||
|
|
@ -1082,9 +1086,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
|
|||
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
|
||||
// Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
|
||||
if (checkForPositionSensorTimeout()) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
} // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_CRUISE, 0, 2);
|
||||
DEBUG_SET(DEBUG_CRUISE, 2, 0);
|
||||
|
|
@ -1153,7 +1158,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(naviga
|
|||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
UNUSED(previousState);
|
||||
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
|
||||
// Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
|
||||
|
|
@ -1169,17 +1174,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
|
||||
// If we have valid position sensor or configured to ignore it's loss at initial stage - continue
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
// Reset altitude and position controllers if necessary
|
||||
if ((prevFlags & NAV_CTL_POS) == 0) {
|
||||
resetPositionController();
|
||||
}
|
||||
|
||||
// Reset altitude controller if it was not enabled or if we are in terrain follow mode
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0 || posControl.flags.isTerrainFollowEnabled) {
|
||||
// Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
|
||||
resetAltitudeController(false);
|
||||
setupAltitudeController();
|
||||
}
|
||||
// Prepare controllers
|
||||
resetPositionController();
|
||||
resetAltitudeController(false); // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
|
||||
setupAltitudeController();
|
||||
|
||||
// If close to home - reset home position and land
|
||||
if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
|
||||
|
|
@ -1250,7 +1248,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
}
|
||||
|
||||
const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
|
||||
const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
|
||||
const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0f) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
|
||||
|
||||
// If we reached desired initial RTH altitude or we don't want to climb first
|
||||
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == RTH_CLIMB_OFF) || rthAltControlStickOverrideCheck(ROLL) || rthClimbStageActiveAndComplete()) {
|
||||
|
|
@ -1283,17 +1281,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
}
|
||||
|
||||
// Climb to safe altitude and turn to correct direction
|
||||
// Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
|
||||
// it in a reasonable time. Immediately after we finish this phase - target the original altitude.
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
if (navConfig()->general.flags.rth_climb_first == RTH_CLIMB_ON_FW_SPIRAL) {
|
||||
float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
|
||||
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
|
||||
} else {
|
||||
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
|
||||
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
|
||||
}
|
||||
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
|
||||
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
|
||||
} else {
|
||||
// Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
|
||||
// it in a reasonable time. Immediately after we finish this phase - target the original altitude.
|
||||
tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM;
|
||||
|
||||
if (navConfig()->general.flags.rth_tail_first) {
|
||||
|
|
@ -1357,6 +1350,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
if (navConfig()->general.flags.rth_use_linear_descent && navConfig()->general.rth_home_altitude > 0) {
|
||||
// Check linear descent status
|
||||
uint32_t homeDistance = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
|
||||
|
||||
if (homeDistance <= METERS_TO_CENTIMETERS(navConfig()->general.rth_linear_descent_start_distance)) {
|
||||
posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
|
||||
}
|
||||
}
|
||||
|
||||
// If we have position sensor - continue home
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
|
||||
|
|
@ -1396,7 +1398,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
|
|||
// 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.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);
|
||||
updateClimbRateToAltitudeController(0, 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);
|
||||
|
|
@ -1415,21 +1417,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
|
|||
}
|
||||
|
||||
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
|
||||
|
||||
if (navConfig()->general.rth_home_altitude) {
|
||||
float timeToReachHomeAltitude = fabsf(tmpHomePos->z - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate;
|
||||
|
||||
if (timeToReachHomeAltitude < 1) {
|
||||
// we almost reached the target home altitude so set the desired altitude to the desired home altitude
|
||||
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
|
||||
} else {
|
||||
float altitudeChangeDirection = tmpHomePos->z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
|
||||
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
|
||||
}
|
||||
}
|
||||
else {
|
||||
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
|
||||
}
|
||||
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
|
@ -1471,7 +1459,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
|
||||
}
|
||||
|
||||
updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL);
|
||||
updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT);
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
|
@ -1494,7 +1482,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
|
|||
UNUSED(previousState);
|
||||
|
||||
if (STATE(ALTITUDE_CONTROL)) {
|
||||
updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL); // FIXME
|
||||
updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME
|
||||
}
|
||||
|
||||
// Prevent I-terms growing when already landed
|
||||
|
|
@ -1506,34 +1494,29 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (posControl.waypointCount == 0 || !posControl.waypointListValid) {
|
||||
if (!posControl.waypointCount || !posControl.waypointListValid) {
|
||||
return NAV_FSM_EVENT_ERROR;
|
||||
}
|
||||
else {
|
||||
// Prepare controllers
|
||||
resetPositionController();
|
||||
|
||||
// Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
|
||||
resetAltitudeController(false);
|
||||
setupAltitudeController();
|
||||
/*
|
||||
Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
|
||||
Using p3 minimises the risk of saving an invalid counter if a mission is aborted.
|
||||
*/
|
||||
if (posControl.activeWaypointIndex == posControl.startWpIndex || posControl.wpMissionRestart) {
|
||||
setupJumpCounters();
|
||||
posControl.activeWaypointIndex = posControl.startWpIndex;
|
||||
wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
|
||||
}
|
||||
// Prepare controllers
|
||||
resetPositionController();
|
||||
resetAltitudeController(false); // Make sure surface tracking is not enabled - WP uses global altitude, not AGL
|
||||
|
||||
if (navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_SWITCH) {
|
||||
posControl.wpMissionRestart = posControl.activeWaypointIndex > posControl.startWpIndex ? !posControl.wpMissionRestart : false;
|
||||
} else {
|
||||
posControl.wpMissionRestart = navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_START;
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
|
||||
if (posControl.activeWaypointIndex == posControl.startWpIndex || posControl.wpMissionRestart) {
|
||||
/* Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
|
||||
Using p3 minimises the risk of saving an invalid counter if a mission is aborted */
|
||||
setupJumpCounters();
|
||||
posControl.activeWaypointIndex = posControl.startWpIndex;
|
||||
wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
|
||||
}
|
||||
|
||||
if (navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_SWITCH) {
|
||||
posControl.wpMissionRestart = posControl.activeWaypointIndex > posControl.startWpIndex ? !posControl.wpMissionRestart : false;
|
||||
} else {
|
||||
posControl.wpMissionRestart = navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_START;
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t nextForNonGeoStates(void)
|
||||
|
|
@ -1562,8 +1545,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
|
|||
posControl.wpAltitudeReached = false;
|
||||
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
|
||||
|
||||
// We use p3 as the volatile jump counter (p2 is the static value)
|
||||
case NAV_WP_ACTION_JUMP:
|
||||
// We use p3 as the volatile jump counter (p2 is the static value)
|
||||
if (posControl.waypointList[posControl.activeWaypointIndex].p3 != -1) {
|
||||
if (posControl.waypointList[posControl.activeWaypointIndex].p3 == 0) {
|
||||
resetJumpCounter();
|
||||
|
|
@ -1702,12 +1685,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
|
|||
|
||||
if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
|
||||
// Adjust altitude to waypoint setting
|
||||
if (STATE(AIRPLANE)) {
|
||||
int8_t altitudeChangeDirection = posControl.activeWaypoint.pos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
|
||||
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
|
||||
} else {
|
||||
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z);
|
||||
}
|
||||
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z);
|
||||
|
||||
posControl.wpAltitudeReached = isWaypointAltitudeReached();
|
||||
|
||||
|
|
@ -1721,6 +1699,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
|
|||
timeMs_t currentTime = millis();
|
||||
|
||||
if (posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0 ||
|
||||
posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT ||
|
||||
(posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)) {
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
|
@ -1876,7 +1855,7 @@ static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
|
|||
static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
|
||||
{
|
||||
const timeMs_t currentMillis = millis();
|
||||
navigationFSMState_t previousState;
|
||||
navigationFSMState_t previousState = NAV_STATE_UNDEFINED;
|
||||
static timeMs_t lastStateProcessTime = 0;
|
||||
|
||||
/* Process new injected event if event defined,
|
||||
|
|
@ -2337,16 +2316,15 @@ static void updateDesiredRTHAltitude(void)
|
|||
posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
|
||||
break;
|
||||
|
||||
case NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT:
|
||||
posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
|
||||
posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
|
||||
break;
|
||||
|
||||
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
|
||||
default:
|
||||
posControl.rthState.rthInitialAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
|
||||
posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
|
||||
}
|
||||
|
||||
if ((navConfig()->general.flags.rth_use_linear_descent) && (navConfig()->general.rth_home_altitude > 0) && (navConfig()->general.rth_linear_descent_start_distance == 0) ) {
|
||||
posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
posControl.rthState.rthClimbStageAltitude = posControl.actualState.abs.pos.z;
|
||||
|
|
@ -2626,7 +2604,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
|
|||
* transiton in to turn.
|
||||
* Limited to fixed wing only.
|
||||
* --------------------------------------------------- */
|
||||
bool rthClimbStageActiveAndComplete() {
|
||||
bool rthClimbStageActiveAndComplete(void) {
|
||||
if ((STATE(FIXED_WING_LEGACY) || STATE(AIRPLANE)) && (navConfig()->general.rth_climb_first_stage_altitude > 0)) {
|
||||
if (posControl.actualState.abs.pos.z >= posControl.rthState.rthClimbStageAltitude) {
|
||||
return true;
|
||||
|
|
@ -2789,7 +2767,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
|
|||
|
||||
// Z-position
|
||||
if ((useMask & NAV_POS_UPDATE_Z) != 0) {
|
||||
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller
|
||||
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller
|
||||
posControl.desiredState.pos.z = pos->z;
|
||||
}
|
||||
|
||||
|
|
@ -2880,28 +2858,36 @@ bool isFlightDetected(void)
|
|||
/*-----------------------------------------------------------
|
||||
* Z-position controller
|
||||
*-----------------------------------------------------------*/
|
||||
void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode)
|
||||
void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
|
||||
{
|
||||
#define MIN_TARGET_CLIMB_RATE 100.0f // cm/s
|
||||
|
||||
static timeUs_t lastUpdateTimeUs;
|
||||
timeUs_t currentTimeUs = micros();
|
||||
|
||||
// Terrain following uses different altitude measurement
|
||||
const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
|
||||
|
||||
if (mode == ROC_TO_ALT_RESET) {
|
||||
lastUpdateTimeUs = currentTimeUs;
|
||||
posControl.desiredState.pos.z = altitudeToUse;
|
||||
}
|
||||
else { // ROC_TO_ALT_NORMAL
|
||||
if (mode != ROC_TO_ALT_RESET && desiredClimbRate) {
|
||||
/* ROC_TO_ALT_CONSTANT - constant climb rate
|
||||
* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached
|
||||
* Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC */
|
||||
|
||||
if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
|
||||
const int8_t direction = desiredClimbRate > 0 ? 1 : -1;
|
||||
const float absClimbRate = fabsf(desiredClimbRate);
|
||||
const uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate;
|
||||
const float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
|
||||
0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate);
|
||||
|
||||
desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate);
|
||||
}
|
||||
|
||||
/*
|
||||
* If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
|
||||
* In other words, when altitude is reached, allow it only to shrink
|
||||
*/
|
||||
if (navConfig()->general.max_altitude > 0 &&
|
||||
altitudeToUse >= navConfig()->general.max_altitude &&
|
||||
desiredClimbRate > 0
|
||||
) {
|
||||
if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0) {
|
||||
desiredClimbRate = 0;
|
||||
}
|
||||
|
||||
|
|
@ -2927,9 +2913,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
|
|||
// Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD
|
||||
posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
|
||||
}
|
||||
|
||||
lastUpdateTimeUs = currentTimeUs;
|
||||
} else { // ROC_TO_ALT_RESET or zero desired climbrate
|
||||
posControl.desiredState.pos.z = altitudeToUse;
|
||||
}
|
||||
|
||||
lastUpdateTimeUs = currentTimeUs;
|
||||
}
|
||||
|
||||
static void resetAltitudeController(bool useTerrainFollowing)
|
||||
|
|
@ -3223,7 +3211,7 @@ void loadSelectedMultiMission(uint8_t missionIndex)
|
|||
posControl.geoWaypointCount = 0;
|
||||
|
||||
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
|
||||
if ((missionCount == missionIndex)) {
|
||||
if (missionCount == missionIndex) {
|
||||
/* store details of selected mission: start wp index, mission wp count, geo wp count */
|
||||
if (!(posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI ||
|
||||
posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD ||
|
||||
|
|
@ -3579,6 +3567,8 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
navTargetPosition[X] = lrintf(posControl.desiredState.pos.x);
|
||||
navTargetPosition[Y] = lrintf(posControl.desiredState.pos.y);
|
||||
navTargetPosition[Z] = lrintf(posControl.desiredState.pos.z);
|
||||
|
||||
navDesiredHeading = wrap_36000(posControl.desiredState.yaw);
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -3849,9 +3839,8 @@ bool navigationRequiresTurnAssistance(void)
|
|||
// For airplanes turn assistant is always required when controlling position
|
||||
return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
|
||||
}
|
||||
else {
|
||||
return false;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -4391,7 +4380,7 @@ int32_t navigationGetHomeHeading(void)
|
|||
}
|
||||
|
||||
// returns m/s
|
||||
float calculateAverageSpeed() {
|
||||
float calculateAverageSpeed(void) {
|
||||
float flightTime = getFlightTime();
|
||||
if (flightTime == 0.0f) return 0;
|
||||
return (float)getTotalTravelDistance() / (flightTime * 100);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue