mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 09:45:33 +03:00
Merge branch 'master' into MrD_Add-odometer-to-OSD
This commit is contained in:
Darren Lines
commit
6b71d7790c
264 changed files with 5289 additions and 2021 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"
|
||||
|
|
@ -46,7 +47,7 @@
|
|||
#include "fc/settings.h"
|
||||
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/mixer_profile.h"
|
||||
#include "flight/pid.h"
|
||||
|
||||
#include "io/beeper.h"
|
||||
|
|
@ -68,6 +69,7 @@
|
|||
#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)
|
||||
#define MR_RTH_CLIMB_MARGIN_PERCENT 15 // on high RTH altitudes use even bigger margin - percent of the altitude set
|
||||
#define MR_RTH_LAND_MARGIN_CM 2000 // pause landing if this amount of cm *before* remaining to the home point (2D distance)
|
||||
|
||||
// Planes:
|
||||
#define FW_RTH_CLIMB_OVERSHOOT_CM 100
|
||||
|
|
@ -81,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
|
||||
|
|
@ -101,7 +96,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, 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 = {
|
||||
|
|
@ -158,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
|
||||
|
|
@ -213,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,
|
||||
|
|
@ -314,6 +309,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS
|
|||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState);
|
||||
|
||||
static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
||||
/** Idle state ******************************************************/
|
||||
|
|
@ -334,6 +332,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -428,7 +427,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,
|
||||
|
|
@ -487,7 +486,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,
|
||||
|
|
@ -602,6 +601,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -660,6 +660,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -813,6 +814,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -947,6 +949,52 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
}
|
||||
},
|
||||
|
||||
/** MIXER AUTOMATED TRANSITION mode, alternated althod ***************************************************/
|
||||
[NAV_STATE_MIXERAT_INITIALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_MIXERAT_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_MIXERAT,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_MIXERAT_IN_PROGRESS,
|
||||
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_MIXERAT_IN_PROGRESS] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_MIXERAT,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_MIXERAT_IN_PROGRESS, // re-process the state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_MIXERAT_ABORT,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME] = NAV_STATE_RTH_HEAD_HOME, //switch to its pending state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LANDING, //switch to its pending state
|
||||
}
|
||||
},
|
||||
[NAV_STATE_MIXERAT_ABORT] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_MIXERAT_ABORT,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_MIXERAT_ABORT, //will not switch to its pending state
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
|
||||
}
|
||||
},
|
||||
};
|
||||
|
||||
static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
|
||||
|
|
@ -954,7 +1002,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;
|
||||
}
|
||||
|
|
@ -1061,7 +1109,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;
|
||||
}
|
||||
|
||||
|
|
@ -1073,7 +1121,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;
|
||||
|
||||
|
|
@ -1094,15 +1148,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));
|
||||
|
|
@ -1135,7 +1198,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);
|
||||
|
||||
|
|
@ -1311,8 +1376,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;
|
||||
|
|
@ -1350,6 +1420,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
if (checkMixerATRequired(MIXERAT_REQUEST_RTH) && (calculateDistanceToDestination(&posControl.rthState.homePosition.pos) > (navConfig()->fw.loiter_radius * 3))){
|
||||
return NAV_FSM_EVENT_SWITCH_TO_MIXERAT;
|
||||
}
|
||||
|
||||
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);
|
||||
|
|
@ -1440,12 +1514,22 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
if (checkMixerATRequired(MIXERAT_REQUEST_LAND)){
|
||||
return NAV_FSM_EVENT_SWITCH_TO_MIXERAT;
|
||||
}
|
||||
|
||||
float descentVelLimited = 0;
|
||||
int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z;
|
||||
|
||||
fpVector3_t tmpHomePos = posControl.rthState.homeTmpWaypoint;
|
||||
uint32_t remaning_distance = calculateDistanceToDestination(&tmpHomePos);
|
||||
|
||||
int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z;
|
||||
if(STATE(MULTIROTOR) && (remaning_distance>MR_RTH_LAND_MARGIN_CM)){
|
||||
descentVelLimited = navConfig()->general.land_minalt_vspd;
|
||||
}
|
||||
// A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
|
||||
if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
|
||||
else if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
|
||||
// land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown
|
||||
// Do not allow descent velocity slower than -30cm/s so the landing detector works.
|
||||
descentVelLimited = navConfig()->general.land_minalt_vspd;
|
||||
|
|
@ -1790,8 +1874,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
disarm(DISARM_NAVIGATION);
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
|
|
@ -1840,6 +1922,70 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi
|
|||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE;
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
|
||||
// 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)) {
|
||||
resetAltitudeController(false);
|
||||
setupAltitudeController();
|
||||
}
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
|
||||
navMixerATPendingState = previousState;
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
mixerProfileATRequest_e required_action;
|
||||
switch (navMixerATPendingState)
|
||||
{
|
||||
case NAV_STATE_RTH_HEAD_HOME:
|
||||
required_action = MIXERAT_REQUEST_RTH;
|
||||
break;
|
||||
case NAV_STATE_RTH_LANDING:
|
||||
required_action = MIXERAT_REQUEST_LAND;
|
||||
break;
|
||||
default:
|
||||
required_action = MIXERAT_REQUEST_NONE;
|
||||
break;
|
||||
}
|
||||
if (mixerATUpdateState(required_action)){
|
||||
// MixerAT is done, switch to next state
|
||||
resetPositionController();
|
||||
resetAltitudeController(false); // Make sure surface tracking is not enabled uses global altitude, not AGL
|
||||
mixerATUpdateState(MIXERAT_REQUEST_ABORT);
|
||||
switch (navMixerATPendingState)
|
||||
{
|
||||
case NAV_STATE_RTH_HEAD_HOME:
|
||||
setupAltitudeController();
|
||||
return NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME;
|
||||
break;
|
||||
case NAV_STATE_RTH_LANDING:
|
||||
setupAltitudeController();
|
||||
return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING;
|
||||
break;
|
||||
default:
|
||||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
mixerATUpdateState(MIXERAT_REQUEST_ABORT);
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
|
||||
{
|
||||
navigationFSMState_t previousState;
|
||||
|
|
@ -2046,7 +2192,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;
|
||||
|
|
@ -2069,11 +2215,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)) {
|
||||
|
|
@ -2438,32 +2587,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();
|
||||
|
|
@ -2475,15 +2625,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;
|
||||
|
|
@ -2491,19 +2641,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
|
||||
|
||||
|
|
@ -2533,9 +2681,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 +2973,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()) {
|
||||
|
|
@ -3430,33 +3576,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)
|
||||
|
|
@ -3475,29 +3622,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();
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -3605,7 +3744,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;
|
||||
|
|
@ -3630,7 +3769,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;
|
||||
|
||||
|
|
@ -3667,7 +3806,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) {
|
||||
|
|
@ -3780,14 +3919,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
}
|
||||
|
||||
// CRUISE has priority over COURSE_HOLD and AH
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE) && STATE(AIRPLANE)) {
|
||||
if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
|
||||
}
|
||||
|
||||
// PH has priority over COURSE_HOLD
|
||||
// CRUISE has priority on AH
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) {
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) && STATE(AIRPLANE)) {
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
|
||||
}
|
||||
|
|
@ -3854,17 +3993,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)
|
||||
|
|
@ -4189,6 +4327,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
|
||||
|
|
@ -4201,15 +4340,6 @@ void navigationInit(void)
|
|||
/* Use system config */
|
||||
navigationUsePIDs();
|
||||
|
||||
if (
|
||||
mixerConfig()->platformType == PLATFORM_BOAT ||
|
||||
mixerConfig()->platformType == PLATFORM_ROVER ||
|
||||
navConfig()->fw.useFwNavYawControl
|
||||
) {
|
||||
ENABLE_STATE(FW_HEADING_USE_YAW);
|
||||
} else {
|
||||
DISABLE_STATE(FW_HEADING_USE_YAW);
|
||||
}
|
||||
#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
|
||||
/* configure WP missions at boot */
|
||||
#ifdef USE_MULTI_MISSION
|
||||
|
|
@ -4270,7 +4400,7 @@ void abortForcedRTH(void)
|
|||
rthState_e getStateOfForcedRTH(void)
|
||||
{
|
||||
/* If forced RTH activated and in AUTO_RTH or EMERG state */
|
||||
if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG))) {
|
||||
if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT))) {
|
||||
if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
|
||||
return RTH_HAS_LANDED;
|
||||
}
|
||||
|
|
@ -4365,7 +4495,7 @@ bool navigationRTHAllowsLanding(void)
|
|||
|
||||
bool isNavLaunchEnabled(void)
|
||||
{
|
||||
return IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH);
|
||||
return (IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH)) && STATE(AIRPLANE);
|
||||
}
|
||||
|
||||
bool abortLaunchAllowed(void)
|
||||
|
|
@ -4401,3 +4531,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