mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 01:35:35 +03:00
Merge remote-tracking branch 'upstream/master' into abo_emerg_flight_rearm
This commit is contained in:
breadoven
commit
45272d9a02
95 changed files with 3054 additions and 646 deletions
|
|
@ -36,6 +36,7 @@
|
|||
|
||||
#include "fc/fc_core.h"
|
||||
#include "fc/config.h"
|
||||
#include "fc/multifunction.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
|
@ -82,17 +83,10 @@ gpsLocation_t GPS_home;
|
|||
uint32_t GPS_distanceToHome; // distance to home point in meters
|
||||
int16_t GPS_directionToHome; // direction to home point in degrees
|
||||
|
||||
fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
|
||||
|
||||
radar_pois_t radar_pois[RADAR_MAX_POIS];
|
||||
|
||||
#if defined(USE_SAFE_HOME)
|
||||
int8_t safehome_index = -1; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
|
||||
uint32_t safehome_distance = 0; // distance to the nearest safehome
|
||||
fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming
|
||||
bool safehome_applied = false; // whether the safehome has been applied to home.
|
||||
|
||||
PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
|
||||
|
||||
#endif
|
||||
|
||||
// waypoint 254, 255 are special waypoints
|
||||
|
|
@ -102,13 +96,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, 4);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5);
|
||||
|
||||
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,
|
||||
|
|
@ -160,6 +153,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.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,
|
||||
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
|
||||
},
|
||||
|
||||
// MC-specific
|
||||
|
|
@ -177,9 +171,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
|
||||
|
|
@ -214,7 +209,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF
|
||||
.launch_abort_deadband = SETTING_NAV_FW_LAUNCH_ABORT_DEADBAND_DEFAULT, // 100 us
|
||||
|
||||
.cruise_yaw_rate = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT, // 20dps
|
||||
.allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
|
||||
.useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
|
||||
.yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
|
||||
|
|
@ -237,10 +231,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;
|
||||
|
|
@ -430,7 +424,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
|
||||
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_RC_POS | NAV_RC_YAW,
|
||||
.mapToFlightModes = NAV_COURSE_HOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
|
|
@ -489,7 +483,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_ALT | NAV_CTL_POS | NAV_CTL_YAW | 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_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | 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,
|
||||
|
|
@ -956,7 +950,7 @@ static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
|
|||
return navFSM[state].stateFlags;
|
||||
}
|
||||
|
||||
static flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state)
|
||||
flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state)
|
||||
{
|
||||
return navFSM[state].mapToFlightModes;
|
||||
}
|
||||
|
|
@ -1063,7 +1057,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (!STATE(FIXED_WING_LEGACY)) { // Only on FW for now
|
||||
if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control
|
||||
return NAV_FSM_EVENT_ERROR;
|
||||
}
|
||||
|
||||
|
|
@ -1075,7 +1069,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
|
|||
|
||||
resetPositionController();
|
||||
|
||||
posControl.cruise.course = posControl.actualState.cog; // Store the course to follow
|
||||
if (STATE(AIRPLANE)) {
|
||||
posControl.cruise.course = posControl.actualState.cog; // Store the course to follow
|
||||
} else { // Multicopter
|
||||
posControl.cruise.course = posControl.actualState.yaw;
|
||||
posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed);
|
||||
posControl.desiredState.pos = posControl.actualState.abs.pos;
|
||||
}
|
||||
posControl.cruise.previousCourse = posControl.cruise.course;
|
||||
posControl.cruise.lastCourseAdjustmentTime = 0;
|
||||
|
||||
|
|
@ -1096,15 +1096,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
|
|||
DEBUG_SET(DEBUG_CRUISE, 0, 2);
|
||||
DEBUG_SET(DEBUG_CRUISE, 2, 0);
|
||||
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
if (STATE(AIRPLANE) && posControl.flags.isAdjustingPosition) { // inhibit for MR, pitch/roll only adjusts speed using pitch
|
||||
return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ;
|
||||
}
|
||||
|
||||
// User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
|
||||
if (posControl.flags.isAdjustingHeading) {
|
||||
const bool mcRollStickHeadingAdjustmentActive = STATE(MULTIROTOR) && ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband;
|
||||
|
||||
// User demanding yaw -> yaw stick on FW, yaw or roll sticks on MR
|
||||
// We record the desired course and change the desired target in the meanwhile
|
||||
if (posControl.flags.isAdjustingHeading || mcRollStickHeadingAdjustmentActive) {
|
||||
int16_t cruiseYawRate = DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate);
|
||||
int16_t headingAdjustCommand = rcCommand[YAW];
|
||||
if (mcRollStickHeadingAdjustmentActive && ABS(rcCommand[ROLL]) > ABS(headingAdjustCommand)) {
|
||||
headingAdjustCommand = -rcCommand[ROLL];
|
||||
}
|
||||
|
||||
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 rateTarget = scaleRangef((float)headingAdjustCommand, -500.0f, 500.0f, -cruiseYawRate, cruiseYawRate);
|
||||
float centidegsPerIteration = rateTarget * MS2S(timeDifference);
|
||||
posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration);
|
||||
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course));
|
||||
|
|
@ -1137,7 +1146,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n
|
|||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
|
||||
if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control
|
||||
return NAV_FSM_EVENT_ERROR;
|
||||
}
|
||||
|
||||
navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
|
||||
|
||||
|
|
@ -1313,8 +1324,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
|
|||
|
||||
if (posControl.flags.estPosStatus >= EST_USABLE) {
|
||||
const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100;
|
||||
#ifdef USE_MULTI_FUNCTIONS
|
||||
const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK);
|
||||
#else
|
||||
const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL);
|
||||
#endif
|
||||
const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance ||
|
||||
(rthAltControlStickOverrideCheck(ROLL) && !posControl.flags.forcedRTHActivated);
|
||||
(overrideTrackback && !posControl.flags.forcedRTHActivated);
|
||||
|
||||
if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) {
|
||||
posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
|
||||
|
|
@ -1792,8 +1808,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
disarm(DISARM_NAVIGATION);
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
|
|
@ -2048,7 +2062,7 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali
|
|||
/*-----------------------------------------------------------
|
||||
* Processes an update to Z-position and velocity
|
||||
*-----------------------------------------------------------*/
|
||||
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus)
|
||||
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError)
|
||||
{
|
||||
posControl.actualState.abs.pos.z = newAltitude;
|
||||
posControl.actualState.abs.vel.z = newVelocity;
|
||||
|
|
@ -2071,11 +2085,14 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
|
|||
posControl.flags.estAltStatus = EST_TRUSTED;
|
||||
posControl.flags.verticalPositionDataNew = true;
|
||||
posControl.lastValidAltitudeTimeMs = millis();
|
||||
/* flag set if mismatch between relative GPS and estimated altitude exceeds 20m */
|
||||
posControl.flags.gpsCfEstimatedAltitudeMismatch = fabsf(gpsCfEstimatedAltitudeError) > 2000;
|
||||
}
|
||||
else {
|
||||
posControl.flags.estAltStatus = EST_NONE;
|
||||
posControl.flags.estAglStatus = EST_NONE;
|
||||
posControl.flags.verticalPositionDataNew = false;
|
||||
posControl.flags.gpsCfEstimatedAltitudeMismatch = false;
|
||||
}
|
||||
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
|
|
@ -2440,32 +2457,33 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
|
|||
}
|
||||
|
||||
#if defined(USE_SAFE_HOME)
|
||||
|
||||
void checkSafeHomeState(bool shouldBeEnabled)
|
||||
{
|
||||
const bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF ||
|
||||
posControl.flags.rthTrackbackActive ||
|
||||
(!safehome_applied && posControl.homeDistance < navConfig()->general.min_rth_distance);
|
||||
bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF || posControl.flags.rthTrackbackActive ||
|
||||
(!posControl.safehomeState.isApplied && posControl.homeDistance < navConfig()->general.min_rth_distance);
|
||||
#ifdef USE_MULTI_FUNCTIONS
|
||||
safehomeNotApplicable = safehomeNotApplicable || (MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated);
|
||||
#endif
|
||||
|
||||
if (safehomeNotApplicable) {
|
||||
shouldBeEnabled = false;
|
||||
} else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) {
|
||||
// if safehomes are only used with failsafe and we're trying to enable safehome
|
||||
// then enable the safehome only with failsafe
|
||||
shouldBeEnabled = posControl.flags.forcedRTHActivated;
|
||||
}
|
||||
if (safehomeNotApplicable) {
|
||||
shouldBeEnabled = false;
|
||||
} else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) {
|
||||
// if safehomes are only used with failsafe and we're trying to enable safehome
|
||||
// then enable the safehome only with failsafe
|
||||
shouldBeEnabled = posControl.flags.forcedRTHActivated;
|
||||
}
|
||||
// no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything
|
||||
if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) {
|
||||
if (posControl.safehomeState.distance == 0 || posControl.safehomeState.isApplied == shouldBeEnabled) {
|
||||
return;
|
||||
}
|
||||
if (shouldBeEnabled) {
|
||||
// set home to safehome
|
||||
setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
|
||||
safehome_applied = true;
|
||||
setHomePosition(&posControl.safehomeState.nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
|
||||
posControl.safehomeState.isApplied = true;
|
||||
} else {
|
||||
// set home to original arming point
|
||||
setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
|
||||
safehome_applied = false;
|
||||
setHomePosition(&posControl.rthState.originalHomePosition, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
|
||||
posControl.safehomeState.isApplied = false;
|
||||
}
|
||||
// if we've changed the home position, update the distance and direction
|
||||
updateHomePosition();
|
||||
|
|
@ -2477,15 +2495,15 @@ void checkSafeHomeState(bool shouldBeEnabled)
|
|||
**********************************************************/
|
||||
bool findNearestSafeHome(void)
|
||||
{
|
||||
safehome_index = -1;
|
||||
posControl.safehomeState.index = -1;
|
||||
uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1;
|
||||
uint32_t distance_to_current;
|
||||
fpVector3_t currentSafeHome;
|
||||
gpsLocation_t shLLH;
|
||||
shLLH.alt = 0;
|
||||
for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
|
||||
if (!safeHomeConfig(i)->enabled)
|
||||
continue;
|
||||
if (!safeHomeConfig(i)->enabled)
|
||||
continue;
|
||||
|
||||
shLLH.lat = safeHomeConfig(i)->lat;
|
||||
shLLH.lon = safeHomeConfig(i)->lon;
|
||||
|
|
@ -2493,19 +2511,17 @@ bool findNearestSafeHome(void)
|
|||
distance_to_current = calculateDistanceToDestination(¤tSafeHome);
|
||||
if (distance_to_current < nearest_safehome_distance) {
|
||||
// this safehome is the nearest so far - keep track of it.
|
||||
safehome_index = i;
|
||||
posControl.safehomeState.index = i;
|
||||
nearest_safehome_distance = distance_to_current;
|
||||
nearestSafeHome.x = currentSafeHome.x;
|
||||
nearestSafeHome.y = currentSafeHome.y;
|
||||
nearestSafeHome.z = currentSafeHome.z;
|
||||
posControl.safehomeState.nearestSafeHome = currentSafeHome;
|
||||
}
|
||||
}
|
||||
if (safehome_index >= 0) {
|
||||
safehome_distance = nearest_safehome_distance;
|
||||
if (posControl.safehomeState.index >= 0) {
|
||||
posControl.safehomeState.distance = nearest_safehome_distance;
|
||||
} else {
|
||||
safehome_distance = 0;
|
||||
posControl.safehomeState.distance = 0;
|
||||
}
|
||||
return safehome_distance > 0;
|
||||
return posControl.safehomeState.distance > 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -2535,9 +2551,7 @@ void updateHomePosition(void)
|
|||
#endif
|
||||
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
|
||||
// save the current location in case it is replaced by a safehome or HOME_RESET
|
||||
original_rth_home.x = posControl.rthState.homePosition.pos.x;
|
||||
original_rth_home.y = posControl.rthState.homePosition.pos.y;
|
||||
original_rth_home.z = posControl.rthState.homePosition.pos.z;
|
||||
posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -2827,7 +2841,7 @@ void updateLandingStatus(timeMs_t currentTimeMs)
|
|||
}
|
||||
} else if (STATE(LANDING_DETECTED)) {
|
||||
pidResetErrorAccumulators();
|
||||
if (navConfig()->general.flags.disarm_on_landing) {
|
||||
if (navConfig()->general.flags.disarm_on_landing && !FLIGHT_MODE(FAILSAFE_MODE)) {
|
||||
ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
|
||||
disarm(DISARM_LANDING);
|
||||
} else if (!navigationInAutomaticThrottleMode()) {
|
||||
|
|
@ -3447,33 +3461,34 @@ bool isNavHoldPositionActive(void)
|
|||
navigationIsExecutingAnEmergencyLanding();
|
||||
}
|
||||
|
||||
float getActiveWaypointSpeed(void)
|
||||
float getActiveSpeed(void)
|
||||
{
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
// In manual control mode use different cap for speed
|
||||
/* Currently only applicable for multicopter */
|
||||
|
||||
// Speed limit for modes where speed manually controlled
|
||||
if (posControl.flags.isAdjustingPosition || FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
return navConfig()->general.max_manual_speed;
|
||||
}
|
||||
else {
|
||||
uint16_t waypointSpeed = navConfig()->general.auto_speed;
|
||||
|
||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
|
||||
if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) {
|
||||
float wpSpecificSpeed = 0.0f;
|
||||
if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)
|
||||
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time
|
||||
else
|
||||
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case
|
||||
uint16_t waypointSpeed = navConfig()->general.auto_speed;
|
||||
|
||||
if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
|
||||
waypointSpeed = wpSpecificSpeed;
|
||||
} else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) {
|
||||
waypointSpeed = navConfig()->general.max_auto_speed;
|
||||
}
|
||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
|
||||
if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) {
|
||||
float wpSpecificSpeed = 0.0f;
|
||||
if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)
|
||||
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time
|
||||
else
|
||||
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case
|
||||
|
||||
if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
|
||||
waypointSpeed = wpSpecificSpeed;
|
||||
} else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) {
|
||||
waypointSpeed = navConfig()->general.max_auto_speed;
|
||||
}
|
||||
}
|
||||
|
||||
return waypointSpeed;
|
||||
}
|
||||
|
||||
return waypointSpeed;
|
||||
}
|
||||
|
||||
bool isWaypointNavTrackingActive(void)
|
||||
|
|
@ -3492,29 +3507,21 @@ static void processNavigationRCAdjustments(void)
|
|||
{
|
||||
/* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */
|
||||
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
|
||||
if ((navStateFlags & NAV_RC_ALT) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
|
||||
posControl.flags.isAdjustingAltitude = adjustAltitudeFromRCInput();
|
||||
}
|
||||
else {
|
||||
posControl.flags.isAdjustingAltitude = false;
|
||||
}
|
||||
|
||||
if (navStateFlags & NAV_RC_POS) {
|
||||
posControl.flags.isAdjustingPosition = adjustPositionFromRCInput() && !FLIGHT_MODE(FAILSAFE_MODE);
|
||||
if (STATE(MULTIROTOR) && FLIGHT_MODE(FAILSAFE_MODE)) {
|
||||
if (FLIGHT_MODE(FAILSAFE_MODE)) {
|
||||
if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) {
|
||||
resetMulticopterBrakingMode();
|
||||
}
|
||||
}
|
||||
else {
|
||||
posControl.flags.isAdjustingAltitude = false;
|
||||
posControl.flags.isAdjustingPosition = false;
|
||||
posControl.flags.isAdjustingHeading = false;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if ((navStateFlags & NAV_RC_YAW) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
|
||||
posControl.flags.isAdjustingHeading = adjustHeadingFromRCInput();
|
||||
}
|
||||
else {
|
||||
posControl.flags.isAdjustingHeading = false;
|
||||
}
|
||||
posControl.flags.isAdjustingAltitude = (navStateFlags & NAV_RC_ALT) && adjustAltitudeFromRCInput();
|
||||
posControl.flags.isAdjustingPosition = (navStateFlags & NAV_RC_POS) && adjustPositionFromRCInput();
|
||||
posControl.flags.isAdjustingHeading = (navStateFlags & NAV_RC_YAW) && adjustHeadingFromRCInput();
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -3584,6 +3591,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);
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -3620,7 +3629,7 @@ static bool isWaypointMissionValid(void)
|
|||
return posControl.waypointListValid && (posControl.waypointCount > 0);
|
||||
}
|
||||
|
||||
static void checkManualEmergencyLandingControl(void)
|
||||
void checkManualEmergencyLandingControl(bool forcedActivation)
|
||||
{
|
||||
static timeMs_t timeout = 0;
|
||||
static int8_t counter = 0;
|
||||
|
|
@ -3645,7 +3654,7 @@ static void checkManualEmergencyLandingControl(void)
|
|||
}
|
||||
|
||||
// Emergency landing toggled ON or OFF after 5 cycles of Poshold mode @ 1Hz minimum rate
|
||||
if (counter >= 5) {
|
||||
if (counter >= 5 || forcedActivation) {
|
||||
counter = 0;
|
||||
posControl.flags.manualEmergLandActive = !posControl.flags.manualEmergLandActive;
|
||||
|
||||
|
|
@ -3682,7 +3691,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
|
||||
/* Emergency landing controlled manually by rapid switching of Poshold mode.
|
||||
* Landing toggled ON or OFF for each Poshold activation sequence */
|
||||
checkManualEmergencyLandingControl();
|
||||
checkManualEmergencyLandingControl(false);
|
||||
|
||||
/* Emergency landing triggered by failsafe Landing or manually initiated */
|
||||
if (posControl.flags.forcedEmergLandingActivated || posControl.flags.manualEmergLandActive) {
|
||||
|
|
@ -3854,9 +3863,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;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -3870,17 +3878,16 @@ int8_t navigationGetHeadingControlState(void)
|
|||
}
|
||||
|
||||
// For multirotors it depends on navigation system mode
|
||||
// Course hold requires Auto Control to update heading hold target whilst RC adjustment active
|
||||
if (navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) {
|
||||
if (posControl.flags.isAdjustingHeading) {
|
||||
if (posControl.flags.isAdjustingHeading && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
return NAV_HEADING_CONTROL_MANUAL;
|
||||
}
|
||||
else {
|
||||
return NAV_HEADING_CONTROL_AUTO;
|
||||
}
|
||||
}
|
||||
else {
|
||||
return NAV_HEADING_CONTROL_NONE;
|
||||
|
||||
return NAV_HEADING_CONTROL_AUTO;
|
||||
}
|
||||
|
||||
return NAV_HEADING_CONTROL_NONE;
|
||||
}
|
||||
|
||||
bool navigationTerrainFollowingEnabled(void)
|
||||
|
|
@ -4205,6 +4212,7 @@ void navigationInit(void)
|
|||
posControl.wpPlannerActiveWPIndex = 0;
|
||||
posControl.flags.wpMissionPlannerActive = false;
|
||||
posControl.startWpIndex = 0;
|
||||
posControl.safehomeState.isApplied = false;
|
||||
#ifdef USE_MULTI_MISSION
|
||||
posControl.multiMissionCount = 0;
|
||||
#endif
|
||||
|
|
@ -4417,3 +4425,8 @@ bool isAdjustingHeading(void) {
|
|||
int32_t getCruiseHeadingAdjustment(void) {
|
||||
return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse);
|
||||
}
|
||||
|
||||
int32_t navigationGetHeadingError(void)
|
||||
{
|
||||
return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog);
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue