mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-17 21:35:37 +03:00
Remove POSHOLD_2D mode (POSHOLD now implies ALTHOLD); Initial cut on surface tracking modifier
Direct altitude control
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
parent
5227273833
commit
73f773f23e
11 changed files with 231 additions and 263 deletions
|
|
@ -273,7 +273,6 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
|
|||
{"navTgtPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navTgtPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navSurf", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
{"navTgtSurf", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
|
||||
#endif
|
||||
};
|
||||
|
||||
|
|
@ -376,7 +375,6 @@ typedef struct blackboxMainState_s {
|
|||
int16_t navHeading;
|
||||
int16_t navTargetHeading;
|
||||
int16_t navSurface;
|
||||
int16_t navTargetSurface;
|
||||
#endif
|
||||
} blackboxMainState_t;
|
||||
|
||||
|
|
@ -708,7 +706,6 @@ static void writeIntraframe(void)
|
|||
}
|
||||
|
||||
blackboxWriteSignedVB(blackboxCurrent->navSurface);
|
||||
blackboxWriteSignedVB(blackboxCurrent->navTargetSurface);
|
||||
#endif
|
||||
|
||||
//Rotate our history buffers:
|
||||
|
|
@ -879,7 +876,6 @@ static void writeInterframe(void)
|
|||
}
|
||||
|
||||
blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
|
||||
blackboxWriteSignedVB(blackboxCurrent->navTargetSurface - blackboxLast->navTargetSurface);
|
||||
#endif
|
||||
|
||||
//Rotate our history buffers
|
||||
|
|
@ -1192,7 +1188,6 @@ static void loadMainState(timeUs_t currentTimeUs)
|
|||
blackboxCurrent->navTargetPos[i] = navTargetPosition[i];
|
||||
}
|
||||
blackboxCurrent->navSurface = navActualSurface;
|
||||
blackboxCurrent->navTargetSurface = navTargetSurface;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -35,6 +35,8 @@
|
|||
#include "sensors/diagnostics.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
#include "navigation/navigation.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
|
||||
#define BOX_SUFFIX ';'
|
||||
|
|
@ -286,7 +288,6 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
|
|||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)), BOXGCSNAV);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSURFACE)), BOXSURFACE);
|
||||
#ifdef USE_FLM_FLAPERON
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FLAPERON)), BOXFLAPERON);
|
||||
#endif
|
||||
|
|
@ -304,6 +305,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
|
|||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT1)), BOXOSDALT1);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT2)), BOXOSDALT2);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT3)), BOXOSDALT3);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(navigationTerrainFollowingEnabled()), BOXSURFACE);
|
||||
|
||||
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
|
||||
for (uint32_t i = 0; i < activeBoxIdCount; i++) {
|
||||
|
|
|
|||
|
|
@ -1213,6 +1213,9 @@ groups:
|
|||
- name: nav_rth_abort_threshold
|
||||
field: general.rth_abort_threshold
|
||||
max: 65000
|
||||
- name: nav_max_terrain_follow_alt
|
||||
field: general.max_terrain_follow_altitude
|
||||
max: 1000
|
||||
- name: nav_rth_altitude
|
||||
field: general.rth_altitude
|
||||
max: 65000
|
||||
|
|
|
|||
|
|
@ -108,6 +108,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.min_rth_distance = 500, // If closer than 5m - land immediately
|
||||
.rth_altitude = 1000, // 10m
|
||||
.rth_abort_threshold = 50000, // 500m - should be safe for all aircraft
|
||||
.max_terrain_follow_altitude = 100, // max 1m altitude in terrain following mode
|
||||
},
|
||||
|
||||
// MC-specific
|
||||
|
|
@ -158,7 +159,6 @@ int16_t navActualHeading;
|
|||
int16_t navDesiredHeading;
|
||||
int16_t navTargetPosition[3];
|
||||
int32_t navLatestActualPosition[3];
|
||||
int16_t navTargetSurface;
|
||||
int16_t navActualSurface;
|
||||
uint16_t navFlags;
|
||||
uint16_t navEPH;
|
||||
|
|
@ -167,10 +167,10 @@ int16_t navAccNEU[3];
|
|||
#endif
|
||||
|
||||
static void updateDesiredRTHAltitude(void);
|
||||
static void resetAltitudeController(void);
|
||||
static void resetAltitudeController(bool useTerrainFollowing);
|
||||
static void resetPositionController(void);
|
||||
static void setupAltitudeController(void);
|
||||
void resetNavigation(void);
|
||||
static void resetHeadingController(void);
|
||||
void resetGCSFlags(void);
|
||||
|
||||
static bool posEstimationHasGlobalReference(void);
|
||||
|
|
@ -186,8 +186,6 @@ bool validateRTHSanityChecker(void);
|
|||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState);
|
||||
|
|
@ -222,7 +220,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
|
|
@ -256,40 +253,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_ALTHOLD_IN_PROGRESS, // re-process the state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
/** POSHOLD_2D mode ************************************************/
|
||||
[NAV_STATE_POSHOLD_2D_INITIALIZE] = {
|
||||
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_POS | NAV_REQUIRE_ANGLE,
|
||||
.mapToFlightModes = NAV_POSHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_HOLD_INFINIT,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_POSHOLD_2D_IN_PROGRESS,
|
||||
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_POSHOLD_2D_IN_PROGRESS] = {
|
||||
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
|
||||
.mapToFlightModes = NAV_POSHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_HOLD_INFINIT,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_POSHOLD_2D_IN_PROGRESS, // re-process the state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[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_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
|
|
@ -323,7 +286,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, // re-process the state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
|
|
@ -359,7 +321,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HEAD_HOME,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
|
@ -376,7 +337,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_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,
|
||||
}
|
||||
|
|
@ -394,7 +354,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_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,
|
||||
}
|
||||
|
|
@ -412,7 +371,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHING,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_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,
|
||||
}
|
||||
|
|
@ -442,7 +400,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_FINISHED, // re-process state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_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,
|
||||
}
|
||||
|
|
@ -491,7 +448,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_REACHED, // successfully reached waypoint
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
|
|
@ -512,7 +468,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
|
|
@ -531,7 +486,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
|
|
@ -561,7 +515,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.onEvent = {
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
|
|
@ -672,27 +625,39 @@ navigationFSMStateFlags_t navGetCurrentStateFlags(void)
|
|||
return navGetStateFlags(posControl.navState);
|
||||
}
|
||||
|
||||
static bool navTerrainFollowingRequested(void)
|
||||
{
|
||||
// Terrain following not supported on FIXED WING aircraft yet
|
||||
return !STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXSURFACE);
|
||||
}
|
||||
|
||||
/*************************************************************************************************/
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
resetNavigation();
|
||||
|
||||
resetAltitudeController(false);
|
||||
resetHeadingController();
|
||||
resetPositionController();
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
const bool terrainFollowingToggled = (posControl.flags.isTerrainFollowEnabled != navTerrainFollowingRequested());
|
||||
|
||||
resetGCSFlags();
|
||||
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0) {
|
||||
resetAltitudeController();
|
||||
// If surface tracking mode changed value - reset altitude controller
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
|
||||
resetAltitudeController(navTerrainFollowingRequested());
|
||||
}
|
||||
|
||||
if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
|
||||
if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0) || terrainFollowingToggled) {
|
||||
setupAltitudeController();
|
||||
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
|
|
@ -704,41 +669,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(nav
|
|||
|
||||
// If GCS was disabled - reset altitude setpoint
|
||||
if (posControl.flags.isGCSAssistedNavigationReset) {
|
||||
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
|
||||
resetGCSFlags();
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
|
||||
resetGCSFlags();
|
||||
|
||||
if ((prevFlags & NAV_CTL_POS) == 0) {
|
||||
resetPositionController();
|
||||
}
|
||||
|
||||
if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
|
||||
fpVector3_t targetHoldPos;
|
||||
calculateInitialHoldPosition(&targetHoldPos);
|
||||
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
// If GCS was disabled - reset 2D pos setpoint
|
||||
if (posControl.flags.isGCSAssistedNavigationReset) {
|
||||
fpVector3_t targetHoldPos;
|
||||
calculateInitialHoldPosition(&targetHoldPos);
|
||||
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
|
||||
resetGCSFlags();
|
||||
}
|
||||
|
||||
|
|
@ -748,6 +679,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(
|
|||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
const bool terrainFollowingToggled = (posControl.flags.isTerrainFollowEnabled != navTerrainFollowingRequested());
|
||||
|
||||
resetGCSFlags();
|
||||
|
||||
|
|
@ -755,13 +687,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
|
|||
resetPositionController();
|
||||
}
|
||||
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0) {
|
||||
resetAltitudeController();
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
|
||||
resetAltitudeController(navTerrainFollowingRequested());
|
||||
setupAltitudeController();
|
||||
}
|
||||
|
||||
if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
|
||||
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
|
||||
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 (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
|
||||
|
|
@ -781,7 +713,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
|
|||
if (posControl.flags.isGCSAssistedNavigationReset) {
|
||||
fpVector3_t targetHoldPos;
|
||||
calculateInitialHoldPosition(&targetHoldPos);
|
||||
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
|
||||
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
resetGCSFlags();
|
||||
}
|
||||
|
|
@ -805,21 +737,23 @@ 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.estPosStatue >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
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();
|
||||
}
|
||||
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0) {
|
||||
resetAltitudeController();
|
||||
// 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();
|
||||
}
|
||||
|
||||
// If close to home - reset home position and land
|
||||
if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
|
||||
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
|
||||
return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
|
||||
}
|
||||
|
|
@ -862,15 +796,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
}
|
||||
|
||||
// If we have valid pos sensor OR we are configured to ignore GPS loss
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
const float rthAltitudeMargin = STATE(FIXED_WING) ?
|
||||
MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)) : // Airplane
|
||||
MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)); // Copters
|
||||
|
||||
if (((posControl.actualState.pos.z - posControl.homeWaypointAbove.pos.z) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) {
|
||||
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homeWaypointAbove.pos.z) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) {
|
||||
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
|
||||
if (STATE(FIXED_WING)) {
|
||||
initializeRTHSanityChecker(&posControl.actualState.pos);
|
||||
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME
|
||||
|
|
@ -922,7 +856,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
}
|
||||
|
||||
// If we have position sensor - continue home
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
if (isWaypointReached(&posControl.homeWaypointAbove, true)) {
|
||||
// Successfully reached position target - update XYZ-position
|
||||
setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
|
|
@ -962,7 +896,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
|
|||
}
|
||||
|
||||
// If position ok OR within valid timeout - continue
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) || !checkForPositionSensorTimeout()) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
|
||||
// Wait until target heading is reached (with 15 deg margin for error)
|
||||
if (STATE(FIXED_WING)) {
|
||||
resetLandingDetector();
|
||||
|
|
@ -1008,14 +942,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
float descentVelLimited = 0;
|
||||
|
||||
// A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
|
||||
if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && posControl.actualState.surface < 50.0f) {
|
||||
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 = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
|
||||
}
|
||||
else {
|
||||
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
|
||||
float descentVelScaling = (posControl.actualState.pos.z - posControl.homePosition.pos.z - navConfig()->general.land_slowdown_minalt)
|
||||
float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homePosition.pos.z - navConfig()->general.land_slowdown_minalt)
|
||||
/ (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt
|
||||
|
||||
descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
|
||||
|
|
@ -1060,7 +994,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
|
|||
else {
|
||||
// Prepare controllers
|
||||
resetPositionController();
|
||||
resetAltitudeController();
|
||||
|
||||
// Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
|
||||
resetAltitudeController(false);
|
||||
setupAltitudeController();
|
||||
|
||||
posControl.activeWaypointIndex = 0;
|
||||
|
|
@ -1080,7 +1016,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
|
|||
|
||||
case NAV_WP_ACTION_RTH:
|
||||
default:
|
||||
initializeRTHSanityChecker(&posControl.actualState.pos);
|
||||
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
|
||||
calcualteAndSetActiveWaypointToLocalPosition(&posControl.homeWaypointAbove.pos);
|
||||
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
|
||||
};
|
||||
|
|
@ -1091,7 +1027,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
|
|||
UNUSED(previousState);
|
||||
|
||||
// If no position sensor available - land immediately
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
const bool isDoingRTH = (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH);
|
||||
|
||||
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
|
||||
|
|
@ -1175,7 +1111,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
|
|||
UNUSED(previousState);
|
||||
|
||||
// If no position sensor available - land immediately
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
/* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
|
||||
|
|
@ -1193,7 +1129,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITI
|
|||
UNUSED(previousState);
|
||||
|
||||
// Emergency landing MAY use common altitude controller if vertical position is valid - initialize it
|
||||
resetAltitudeController();
|
||||
// Make sure terrain following is not enabled
|
||||
resetAltitudeController(false);
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
|
@ -1454,7 +1391,7 @@ bool isThrustFacingDownwards(void)
|
|||
bool checkForPositionSensorTimeout(void)
|
||||
{
|
||||
if (navConfig()->general.pos_failure_timeout) {
|
||||
if ((posControl.flags.estPosStatue == EST_NONE) && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) {
|
||||
if ((posControl.flags.estPosStatus == EST_NONE) && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
|
|
@ -1472,20 +1409,25 @@ bool checkForPositionSensorTimeout(void)
|
|||
*-----------------------------------------------------------*/
|
||||
void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY)
|
||||
{
|
||||
posControl.actualState.pos.x = newX;
|
||||
posControl.actualState.pos.y = newY;
|
||||
posControl.actualState.abs.pos.x = newX;
|
||||
posControl.actualState.abs.pos.y = newY;
|
||||
posControl.actualState.abs.vel.x = newVelX;
|
||||
posControl.actualState.abs.vel.y = newVelY;
|
||||
|
||||
posControl.actualState.agl.pos.x = newX;
|
||||
posControl.actualState.agl.pos.y = newY;
|
||||
posControl.actualState.agl.vel.x = newVelX;
|
||||
posControl.actualState.agl.vel.y = newVelY;
|
||||
|
||||
posControl.actualState.vel.x = newVelX;
|
||||
posControl.actualState.vel.y = newVelY;
|
||||
posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));
|
||||
|
||||
if (estimateValid) {
|
||||
posControl.flags.estPosStatue = EST_TRUSTED;
|
||||
posControl.flags.estPosStatus = EST_TRUSTED;
|
||||
posControl.flags.horizontalPositionDataNew = 1;
|
||||
posControl.lastValidPositionTimeMs = millis();
|
||||
}
|
||||
else {
|
||||
posControl.flags.estPosStatue = EST_NONE;
|
||||
posControl.flags.estPosStatus = EST_NONE;
|
||||
posControl.flags.horizontalPositionDataNew = 0;
|
||||
}
|
||||
|
||||
|
|
@ -1502,40 +1444,41 @@ void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, f
|
|||
*-----------------------------------------------------------*/
|
||||
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus)
|
||||
{
|
||||
posControl.actualState.pos.z = newAltitude;
|
||||
posControl.actualState.vel.z = newVelocity;
|
||||
posControl.actualState.surface = surfaceDistance;
|
||||
posControl.actualState.surfaceVel = surfaceVelocity;
|
||||
posControl.actualState.abs.pos.z = newAltitude;
|
||||
posControl.actualState.abs.vel.z = newVelocity;
|
||||
|
||||
posControl.actualState.agl.pos.z = surfaceDistance;
|
||||
posControl.actualState.agl.vel.z = surfaceVelocity;
|
||||
|
||||
// Update altitude that would be used when executing RTH
|
||||
if (estimateValid) {
|
||||
updateDesiredRTHAltitude();
|
||||
|
||||
// If we acquired new surface reference - changing from NONE/USABLE -> TRUSTED
|
||||
if ((surfaceStatus == EST_TRUSTED) && (posControl.flags.estSurfaceStatus != EST_TRUSTED)) {
|
||||
if ((surfaceStatus == EST_TRUSTED) && (posControl.flags.estAglStatus != EST_TRUSTED)) {
|
||||
// If we are in terrain-following modes - signal that we should update the surface tracking setpoint
|
||||
// NONE/USABLE means that we were flying blind, now we should lock to surface
|
||||
//updateSurfaceTrackingSetpoint();
|
||||
}
|
||||
|
||||
posControl.flags.estSurfaceStatus = surfaceStatus; // Could be TRUSTED or USABLE
|
||||
posControl.flags.estAglStatus = surfaceStatus; // Could be TRUSTED or USABLE
|
||||
posControl.flags.estAltStatus = EST_TRUSTED;
|
||||
posControl.flags.verticalPositionDataNew = 1;
|
||||
posControl.lastValidAltitudeTimeMs = millis();
|
||||
}
|
||||
else {
|
||||
posControl.flags.estAltStatus = EST_NONE;
|
||||
posControl.flags.estSurfaceStatus = EST_NONE;
|
||||
posControl.flags.estAglStatus = EST_NONE;
|
||||
posControl.flags.verticalPositionDataNew = 0;
|
||||
}
|
||||
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && posControl.actualState.surface > 0) {
|
||||
if ((posControl.flags.estAglStatus == EST_TRUSTED) && surfaceDistance > 0) {
|
||||
if (posControl.actualState.surfaceMin > 0) {
|
||||
posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, posControl.actualState.surface);
|
||||
posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, surfaceDistance);
|
||||
}
|
||||
else {
|
||||
posControl.actualState.surfaceMin = posControl.actualState.surface;
|
||||
posControl.actualState.surfaceMin = surfaceDistance;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1544,8 +1487,8 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
|
|||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navLatestActualPosition[Z] = constrain(posControl.actualState.pos.z, -32678, 32767);
|
||||
navActualVelocity[Z] = constrain(posControl.actualState.vel.z, -32678, 32767);
|
||||
navLatestActualPosition[Z] = constrain(navGetCurrentActualPositionAndVelocity()->pos.z, -32678, 32767);
|
||||
navActualVelocity[Z] = constrain(navGetCurrentActualPositionAndVelocity()->vel.z, -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -1565,21 +1508,29 @@ void updateActualHeading(bool headingValid, int32_t newHeading)
|
|||
posControl.flags.headingDataNew = 1;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Returns pointer to currently used position (ABS or AGL) depending on surface tracking status
|
||||
*-----------------------------------------------------------*/
|
||||
const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
|
||||
{
|
||||
return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Calculates distance and bearing to destination point
|
||||
*-----------------------------------------------------------*/
|
||||
uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos)
|
||||
{
|
||||
const float deltaX = destinationPos->x - posControl.actualState.pos.x;
|
||||
const float deltaY = destinationPos->y - posControl.actualState.pos.y;
|
||||
const float deltaX = destinationPos->x - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
const float deltaY = destinationPos->y - navGetCurrentActualPositionAndVelocity()->pos.y;
|
||||
|
||||
return sqrtf(sq(deltaX) + sq(deltaY));
|
||||
}
|
||||
|
||||
int32_t calculateBearingToDestination(const fpVector3_t * destinationPos)
|
||||
{
|
||||
const float deltaX = destinationPos->x - posControl.actualState.pos.x;
|
||||
const float deltaY = destinationPos->y - posControl.actualState.pos.y;
|
||||
const float deltaX = destinationPos->x - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
const float deltaY = destinationPos->y - navGetCurrentActualPositionAndVelocity()->pos.y;
|
||||
|
||||
return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX)));
|
||||
}
|
||||
|
|
@ -1626,16 +1577,16 @@ static void updateDesiredRTHAltitude(void)
|
|||
if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
|
||||
switch (navConfig()->general.flags.rth_alt_control_mode) {
|
||||
case NAV_RTH_NO_ALT:
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.pos.z;
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z;
|
||||
break;
|
||||
case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.pos.z + navConfig()->general.rth_altitude;
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude;
|
||||
break;
|
||||
case NAV_RTH_MAX_ALT:
|
||||
posControl.homeWaypointAbove.pos.z = MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.pos.z);
|
||||
posControl.homeWaypointAbove.pos.z = MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z);
|
||||
break;
|
||||
case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
|
||||
posControl.homeWaypointAbove.pos.z = MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.pos.z);
|
||||
posControl.homeWaypointAbove.pos.z = MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
|
||||
break;
|
||||
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
|
||||
default:
|
||||
|
|
@ -1645,7 +1596,7 @@ static void updateDesiredRTHAltitude(void)
|
|||
}
|
||||
}
|
||||
else {
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.pos.z;
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1731,7 +1682,7 @@ void updateHomePosition(void)
|
|||
{
|
||||
// Disarmed and have a valid position, constantly update home
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
if (posControl.flags.estPosStatue >= EST_USABLE) {
|
||||
if (posControl.flags.estPosStatus >= EST_USABLE) {
|
||||
bool setHome = !posControl.flags.isHomeValid;
|
||||
switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) {
|
||||
case NAV_RESET_NEVER:
|
||||
|
|
@ -1744,7 +1695,7 @@ void updateHomePosition(void)
|
|||
break;
|
||||
}
|
||||
if (setHome) {
|
||||
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1753,9 +1704,9 @@ void updateHomePosition(void)
|
|||
|
||||
// If pilot so desires he may reset home position to current position
|
||||
if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
|
||||
if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, homeUpdateFlags);
|
||||
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags);
|
||||
isHomeResetAllowed = false;
|
||||
}
|
||||
}
|
||||
|
|
@ -1819,7 +1770,6 @@ 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
|
||||
posControl.desiredState.surface = -1; // When we directly set altitude target we must reset surface tracking
|
||||
posControl.desiredState.pos.z = pos->z;
|
||||
}
|
||||
|
||||
|
|
@ -1838,9 +1788,9 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
|
|||
|
||||
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance)
|
||||
{
|
||||
farAwayPos->x = posControl.actualState.pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
farAwayPos->y = posControl.actualState.pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
farAwayPos->z = posControl.actualState.pos.z;
|
||||
farAwayPos->x = navGetCurrentActualPositionAndVelocity()->pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
farAwayPos->y = navGetCurrentActualPositionAndVelocity()->pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -1878,9 +1828,12 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
|
|||
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 = posControl.actualState.pos.z;
|
||||
posControl.desiredState.pos.z = altitudeToUse;
|
||||
}
|
||||
else {
|
||||
if (STATE(FIXED_WING)) {
|
||||
|
|
@ -1892,22 +1845,23 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
|
|||
|
||||
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
|
||||
posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
|
||||
posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, // FIXME: calculate sanity limits in a smarter way
|
||||
posControl.actualState.pos.z - 500,
|
||||
posControl.actualState.pos.z + 500);
|
||||
posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500); // FIXME: calculate sanity limits in a smarter way
|
||||
}
|
||||
}
|
||||
else {
|
||||
// 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 = posControl.actualState.pos.z + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
|
||||
posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
|
||||
}
|
||||
|
||||
lastUpdateTimeUs = currentTimeUs;
|
||||
}
|
||||
}
|
||||
|
||||
static void resetAltitudeController(void)
|
||||
static void resetAltitudeController(bool useTerrainFollowing)
|
||||
{
|
||||
// Set terrain following flag
|
||||
posControl.flags.isTerrainFollowEnabled = useTerrainFollowing;
|
||||
|
||||
if (STATE(FIXED_WING)) {
|
||||
resetFixedWingAltitudeController();
|
||||
}
|
||||
|
|
@ -2019,7 +1973,7 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
|
|||
else if (wpNumber == 255) {
|
||||
gpsLocation_t wpLLH;
|
||||
|
||||
geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.actualState.pos, &wpLLH);
|
||||
geoConvertLocalToGeodetic(&posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos, &wpLLH);
|
||||
|
||||
wpData->lat = wpLLH.lat;
|
||||
wpData->lon = wpLLH.lon;
|
||||
|
|
@ -2044,7 +1998,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
|
|||
wpLLH.alt = wpData->alt;
|
||||
|
||||
// WP #0 - special waypoint - HOME
|
||||
if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatue >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
|
||||
if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
|
||||
// Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
|
||||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
|
||||
setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
|
|
@ -2052,8 +2006,8 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
|
|||
// WP #255 - special waypoint - directly set desiredPosition
|
||||
// Only valid when armed and in poshold mode
|
||||
else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
|
||||
ARMING_FLAG(ARMED) && (posControl.flags.estPosStatue >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
|
||||
(posControl.navState == NAV_STATE_POSHOLD_2D_IN_PROGRESS || posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
|
||||
ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
|
||||
(posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
|
||||
// Convert to local coordinates
|
||||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
|
||||
|
||||
|
|
@ -2221,16 +2175,6 @@ float getActiveWaypointSpeed(void)
|
|||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* A function to reset navigation PIDs and states
|
||||
*-----------------------------------------------------------*/
|
||||
void resetNavigation(void)
|
||||
{
|
||||
resetAltitudeController();
|
||||
resetHeadingController();
|
||||
resetPositionController();
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Process adjustments to alt, pos and yaw controllers
|
||||
*-----------------------------------------------------------*/
|
||||
|
|
@ -2270,8 +2214,8 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
#if defined(NAV_BLACKBOX)
|
||||
navFlags = 0;
|
||||
if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0);
|
||||
if (posControl.flags.estSurfaceStatus == EST_TRUSTED) navFlags |= (1 << 1);
|
||||
if (posControl.flags.estPosStatue == EST_TRUSTED) navFlags |= (1 << 2);
|
||||
if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1);
|
||||
if (posControl.flags.estPosStatus == EST_TRUSTED) navFlags |= (1 << 2);
|
||||
#if defined(NAV_GPS_GLITCH_DETECTION)
|
||||
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
|
||||
#endif
|
||||
|
|
@ -2317,7 +2261,6 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.x), -32678, 32767);
|
||||
navTargetPosition[Y] = constrain(lrintf(posControl.desiredState.pos.y), -32678, 32767);
|
||||
navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.z), -32678, 32767);
|
||||
navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -2342,7 +2285,7 @@ static bool canActivateAltHoldMode(void)
|
|||
|
||||
static bool canActivatePosHoldMode(void)
|
||||
{
|
||||
return (posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
|
||||
return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
|
||||
}
|
||||
|
||||
static bool posEstimationHasGlobalReference(void)
|
||||
|
|
@ -2409,14 +2352,9 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
canActivateWaypoint = true;
|
||||
}
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
|
||||
if ((FLIGHT_MODE(NAV_ALTHOLD_MODE) && FLIGHT_MODE(NAV_POSHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
|
||||
}
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
|
||||
if ((FLIGHT_MODE(NAV_POSHOLD_MODE)) || (canActivatePosHold))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D;
|
||||
if (FLIGHT_MODE(NAV_POSHOLD_MODE) || (canActivatePosHold && canActivateAltHold))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
|
||||
}
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
|
||||
|
|
@ -2490,6 +2428,11 @@ int8_t navigationGetHeadingControlState(void)
|
|||
}
|
||||
}
|
||||
|
||||
bool navigationTerrainFollowingEnabled(void)
|
||||
{
|
||||
return posControl.flags.isTerrainFollowEnabled;
|
||||
}
|
||||
|
||||
bool navigationBlockArming(void)
|
||||
{
|
||||
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD);
|
||||
|
|
@ -2500,7 +2443,7 @@ bool navigationBlockArming(void)
|
|||
return false;
|
||||
|
||||
// Apply extra arming safety only if pilot has any of GPS modes configured
|
||||
if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatue >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
|
||||
if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
|
||||
shouldBlockArming = true;
|
||||
}
|
||||
|
||||
|
|
@ -2527,7 +2470,7 @@ bool navigationBlockArming(void)
|
|||
|
||||
bool navigationPositionEstimateIsHealthy(void)
|
||||
{
|
||||
return (posControl.flags.estPosStatue >= EST_USABLE) && STATE(GPS_FIX_HOME);
|
||||
return (posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -2551,7 +2494,6 @@ void updateFlightBehaviorModifiers(void)
|
|||
}
|
||||
|
||||
posControl.flags.isGCSAssistedNavigationEnabled = IS_RC_MODE_ACTIVE(BOXGCSNAV);
|
||||
posControl.flags.isTerrainFollowEnabled = IS_RC_MODE_ACTIVE(BOXSURFACE);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -2640,9 +2582,9 @@ void navigationInit(void)
|
|||
posControl.flags.headingDataNew = 0;
|
||||
|
||||
posControl.flags.estAltStatus = EST_NONE;
|
||||
posControl.flags.estPosStatue = EST_NONE;
|
||||
posControl.flags.estPosStatus = EST_NONE;
|
||||
posControl.flags.estHeadingStatus = EST_NONE;
|
||||
posControl.flags.estSurfaceStatus = EST_NONE;
|
||||
posControl.flags.estAglStatus = EST_NONE;
|
||||
|
||||
posControl.flags.forcedRTHActivated = 0;
|
||||
posControl.waypointCount = 0;
|
||||
|
|
@ -2650,8 +2592,6 @@ void navigationInit(void)
|
|||
posControl.waypointListValid = false;
|
||||
|
||||
/* Set initial surface invalid */
|
||||
posControl.actualState.surface = -1.0f;
|
||||
posControl.actualState.surfaceVel = 0.0f;
|
||||
posControl.actualState.surfaceMin = -1.0f;
|
||||
|
||||
/* Reset statistics */
|
||||
|
|
@ -2666,12 +2606,12 @@ void navigationInit(void)
|
|||
*-----------------------------------------------------------*/
|
||||
float getEstimatedActualVelocity(int axis)
|
||||
{
|
||||
return posControl.actualState.vel.v[axis];
|
||||
return navGetCurrentActualPositionAndVelocity()->vel.v[axis];
|
||||
}
|
||||
|
||||
float getEstimatedActualPosition(int axis)
|
||||
{
|
||||
return posControl.actualState.pos.v[axis];
|
||||
return navGetCurrentActualPositionAndVelocity()->pos.v[axis];
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -134,6 +134,7 @@ typedef struct navConfig_s {
|
|||
uint16_t rth_altitude; // altitude to maintain when RTH is active (depends on rth_alt_control_mode) (cm)
|
||||
uint16_t min_rth_distance; // 0 Disables. Minimal distance for RTH in cm, otherwise it will just autoland
|
||||
uint16_t rth_abort_threshold; // Initiate emergency landing if during RTH we get this much [cm] away from home
|
||||
uint16_t max_terrain_follow_altitude; // Max altitude to be used in SURFACE TRACKING mode
|
||||
} general;
|
||||
|
||||
struct {
|
||||
|
|
@ -275,6 +276,7 @@ int8_t navigationGetHeadingControlState(void);
|
|||
bool navigationBlockArming(void);
|
||||
bool navigationPositionEstimateIsHealthy(void);
|
||||
bool navIsCalibrationComplete(void);
|
||||
bool navigationTerrainFollowingEnabled(void);
|
||||
|
||||
/* Access to estimated position and velocity */
|
||||
float getEstimatedActualVelocity(int axis);
|
||||
|
|
@ -335,7 +337,6 @@ extern int16_t navActualVelocity[3];
|
|||
extern int16_t navDesiredVelocity[3];
|
||||
extern int16_t navTargetPosition[3];
|
||||
extern int32_t navLatestActualPosition[3];
|
||||
extern int16_t navTargetSurface;
|
||||
extern int16_t navActualSurface;
|
||||
extern uint16_t navFlags;
|
||||
extern uint16_t navEPH;
|
||||
|
|
|
|||
|
|
@ -107,7 +107,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
|
|||
const float demSPE = (posControl.desiredState.pos.z / 100.0f) * GRAVITY_MSS;
|
||||
const float demSKE = 0.0f;
|
||||
|
||||
const float estSPE = (posControl.actualState.pos.z / 100.0f) * GRAVITY_MSS;
|
||||
const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z / 100.0f) * GRAVITY_MSS;
|
||||
const float estSKE = 0.0f;
|
||||
|
||||
// speedWeight controls balance between potential and kinetic energy used for pitch controller
|
||||
|
|
@ -206,8 +206,8 @@ void resetFixedWingPositionController(void)
|
|||
|
||||
static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
||||
{
|
||||
float posErrorX = posControl.desiredState.pos.x - posControl.actualState.pos.x;
|
||||
float posErrorY = posControl.desiredState.pos.y - posControl.actualState.pos.y;
|
||||
float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
|
||||
|
||||
float distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
|
||||
|
||||
|
|
@ -229,14 +229,14 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
|||
float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle);
|
||||
|
||||
// We have temporary loiter target. Recalculate distance and position error
|
||||
posErrorX = loiterTargetX - posControl.actualState.pos.x;
|
||||
posErrorY = loiterTargetY - posControl.actualState.pos.y;
|
||||
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
posErrorY = loiterTargetY - navGetCurrentActualPositionAndVelocity()->pos.y;
|
||||
distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
|
||||
}
|
||||
|
||||
// Calculate virtual waypoint
|
||||
virtualDesiredPosition.x = posControl.actualState.pos.x + posErrorX * (trackingDistance / distanceToActualTarget);
|
||||
virtualDesiredPosition.y = posControl.actualState.pos.y + posErrorY * (trackingDistance / distanceToActualTarget);
|
||||
virtualDesiredPosition.x = navGetCurrentActualPositionAndVelocity()->pos.x + posErrorX * (trackingDistance / distanceToActualTarget);
|
||||
virtualDesiredPosition.y = navGetCurrentActualPositionAndVelocity()->pos.y + posErrorY * (trackingDistance / distanceToActualTarget);
|
||||
|
||||
// Shift position according to pilot's ROLL input (up to max_manual_speed velocity)
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
|
|
@ -330,7 +330,7 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
// Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
// If we have new position - update velocity and acceleration controllers
|
||||
if (posControl.flags.horizontalPositionDataNew) {
|
||||
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
|
||||
|
|
@ -378,7 +378,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
// Apply controller only if position source is valid
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
// If we have new position - update velocity and acceleration controllers
|
||||
if (posControl.flags.horizontalPositionDataNew) {
|
||||
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
|
||||
|
|
@ -473,8 +473,8 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
* TODO refactor conditions in this metod if logic is proven to be correct
|
||||
*/
|
||||
if (navStateFlags & NAV_CTL_LAND) {
|
||||
if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (posControl.actualState.pos.z <= navConfig()->general.land_slowdown_minalt)) ||
|
||||
((posControl.flags.estSurfaceStatus == EST_TRUSTED) && (posControl.actualState.surface <= navConfig()->general.land_slowdown_minalt)) ) {
|
||||
if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (navGetCurrentActualPositionAndVelocity()->pos.z <= navConfig()->general.land_slowdown_minalt)) ||
|
||||
((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) ) {
|
||||
/*
|
||||
* Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
|
||||
*/
|
||||
|
|
@ -534,7 +534,7 @@ void applyFixedWingEmergencyLandingController(void)
|
|||
void calculateFixedWingInitialHoldPosition(fpVector3_t * pos)
|
||||
{
|
||||
// TODO: stub, this should account for velocity and target loiter radius
|
||||
*pos = posControl.actualState.pos;
|
||||
*pos = navGetCurrentActualPositionAndVelocity()->pos;
|
||||
}
|
||||
|
||||
void resetFixedWingHeadingController(void)
|
||||
|
|
|
|||
|
|
@ -61,7 +61,7 @@ static bool prepareForTakeoffOnReset = false;
|
|||
// Position to velocity controller for Z axis
|
||||
static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
|
||||
{
|
||||
const float altitudeError = posControl.desiredState.pos.z - posControl.actualState.pos.z;
|
||||
const float altitudeError = posControl.desiredState.pos.z - navGetCurrentActualPositionAndVelocity()->pos.z;
|
||||
float targetVel = altitudeError * posControl.pids.pos[Z].param.kP;
|
||||
|
||||
// hard limit desired target velocity to max_climb_rate
|
||||
|
|
@ -93,7 +93,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
|
|||
const int16_t thrAdjustmentMin = (int16_t)motorConfig()->minthrottle - (int16_t)navConfig()->mc.hover_throttle;
|
||||
const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)navConfig()->mc.hover_throttle;
|
||||
|
||||
posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, posControl.actualState.vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
|
||||
posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
|
||||
|
||||
posControl.rcAdjustment[THROTTLE] = pt1FilterApply4(&altholdThrottleFilterState, posControl.rcAdjustment[THROTTLE], NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
|
||||
posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax);
|
||||
|
|
@ -101,6 +101,23 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
|
|||
|
||||
bool adjustMulticopterAltitudeFromRCInput(void)
|
||||
{
|
||||
if (posControl.flags.isTerrainFollowEnabled) {
|
||||
const float altTarget = scaleRangef(rcCommand[THROTTLE], motorConfig()->minthrottle, motorConfig()->maxthrottle, 0, navConfig()->general.max_terrain_follow_altitude);
|
||||
|
||||
// In terrain follow mode we apply different logic for terrain control
|
||||
if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) {
|
||||
// We have solid terrain sensor signal - directly map throttle to altitude
|
||||
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
|
||||
posControl.desiredState.pos.z = altTarget;
|
||||
}
|
||||
else {
|
||||
updateClimbRateToAltitudeController(-50.0f, ROC_TO_ALT_NORMAL);
|
||||
}
|
||||
|
||||
// In surface tracking we always indicate that we're adjusting altitude
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
const int16_t rcThrottleAdjustment = applyDeadband(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband);
|
||||
if (rcThrottleAdjustment) {
|
||||
// set velocity proportional to stick movement
|
||||
|
|
@ -128,6 +145,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
|
|||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void setupMulticopterAltitudeController(void)
|
||||
|
|
@ -160,6 +178,8 @@ void setupMulticopterAltitudeController(void)
|
|||
|
||||
void resetMulticopterAltitudeController(void)
|
||||
{
|
||||
const navEstimatedPosVel_t * posToUse = navGetCurrentActualPositionAndVelocity();
|
||||
|
||||
navPidReset(&posControl.pids.vel[Z]);
|
||||
navPidReset(&posControl.pids.surface);
|
||||
posControl.rcAdjustment[THROTTLE] = 0;
|
||||
|
|
@ -167,13 +187,13 @@ void resetMulticopterAltitudeController(void)
|
|||
if (prepareForTakeoffOnReset) {
|
||||
/* If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump */
|
||||
posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate;
|
||||
posControl.desiredState.pos.z = posControl.actualState.pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
|
||||
posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
|
||||
posControl.pids.vel[Z].integrator = -500.0f;
|
||||
pt1FilterReset(&altholdThrottleFilterState, -500.0f);
|
||||
prepareForTakeoffOnReset = false;
|
||||
}
|
||||
else {
|
||||
posControl.desiredState.vel.z = posControl.actualState.vel.z; // Gradually transition from current climb
|
||||
posControl.desiredState.vel.z = posToUse->vel.z; // Gradually transition from current climb
|
||||
pt1FilterReset(&altholdThrottleFilterState, 0.0f);
|
||||
}
|
||||
}
|
||||
|
|
@ -270,8 +290,8 @@ bool adjustMulticopterPositionFromRCInput(void)
|
|||
const float neuVelY = rcVelX * posControl.actualState.sinYaw + rcVelY * posControl.actualState.cosYaw;
|
||||
|
||||
// Calculate new position target, so Pos-to-Vel P-controller would yield desired velocity
|
||||
posControl.desiredState.pos.x = posControl.actualState.pos.x + (neuVelX / posControl.pids.pos[X].param.kP);
|
||||
posControl.desiredState.pos.y = posControl.actualState.pos.y + (neuVelY / posControl.pids.pos[Y].param.kP);
|
||||
posControl.desiredState.pos.x = navGetCurrentActualPositionAndVelocity()->pos.x + (neuVelX / posControl.pids.pos[X].param.kP);
|
||||
posControl.desiredState.pos.y = navGetCurrentActualPositionAndVelocity()->pos.y + (neuVelY / posControl.pids.pos[Y].param.kP);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
|
@ -314,8 +334,8 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax)
|
|||
|
||||
static void updatePositionVelocityController_MC(void)
|
||||
{
|
||||
const float posErrorX = posControl.desiredState.pos.x - posControl.actualState.pos.x;
|
||||
const float posErrorY = posControl.desiredState.pos.y - posControl.actualState.pos.y;
|
||||
const float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
const float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
|
||||
|
||||
// Calculate target velocity
|
||||
float newVelX = posErrorX * posControl.pids.pos[X].param.kP;
|
||||
|
|
@ -348,8 +368,8 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
|
|||
{
|
||||
|
||||
// Calculate velocity error
|
||||
const float velErrorX = posControl.desiredState.vel.x - posControl.actualState.vel.x;
|
||||
const float velErrorY = posControl.desiredState.vel.y - posControl.actualState.vel.y;
|
||||
const float velErrorX = posControl.desiredState.vel.x - navGetCurrentActualPositionAndVelocity()->vel.x;
|
||||
const float velErrorY = posControl.desiredState.vel.y - navGetCurrentActualPositionAndVelocity()->vel.y;
|
||||
|
||||
// Calculate XY-acceleration limit according to velocity error limit
|
||||
float accelLimitX, accelLimitY;
|
||||
|
|
@ -376,8 +396,8 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
|
|||
// Apply PID with output limiting and I-term anti-windup
|
||||
// Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit
|
||||
// Thus we don't need to do anything else with calculated acceleration
|
||||
const float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.x, posControl.actualState.vel.x, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0);
|
||||
const float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, posControl.actualState.vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0);
|
||||
const float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.x, navGetCurrentActualPositionAndVelocity()->vel.x, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0);
|
||||
const float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, navGetCurrentActualPositionAndVelocity()->vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0);
|
||||
|
||||
// Save last acceleration target
|
||||
lastAccelTargetX = newAccelX;
|
||||
|
|
@ -422,7 +442,7 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
|
|||
|
||||
// Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
|
||||
// and pilots input would be passed thru to PID controller
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
// If we have new position - update velocity and acceleration controllers
|
||||
if (posControl.flags.horizontalPositionDataNew) {
|
||||
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
|
||||
|
|
@ -484,7 +504,7 @@ bool isMulticopterLandingDetected(void)
|
|||
}
|
||||
|
||||
// Average climb rate should be low enough
|
||||
bool verticalMovement = fabsf(posControl.actualState.vel.z) > 25.0f;
|
||||
bool verticalMovement = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) > 25.0f;
|
||||
|
||||
// check if we are moving horizontally
|
||||
bool horizontalMovement = posControl.actualState.velXY > 100.0f;
|
||||
|
|
@ -507,11 +527,11 @@ bool isMulticopterLandingDetected(void)
|
|||
DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 2, (currentTimeUs - landingTimer) / 1000);
|
||||
|
||||
// If we have surface sensor (for example sonar) - use it to detect touchdown
|
||||
if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && (posControl.actualState.surfaceMin >= 0)) {
|
||||
if ((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z >= 0)) {
|
||||
// TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety.
|
||||
// TODO: Out of range sonar may give reading that looks like we landed, find a way to check if sonar is healthy.
|
||||
// surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed
|
||||
possibleLandingDetected = possibleLandingDetected && (posControl.actualState.surface <= (posControl.actualState.surfaceMin + 5.0f));
|
||||
possibleLandingDetected = possibleLandingDetected && (posControl.actualState.agl.pos.z <= (posControl.actualState.surfaceMin + 5.0f));
|
||||
}
|
||||
|
||||
if (!possibleLandingDetected) {
|
||||
|
|
@ -585,11 +605,11 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
|||
*-----------------------------------------------------------*/
|
||||
void calculateMulticopterInitialHoldPosition(fpVector3_t * pos)
|
||||
{
|
||||
const float stoppingDistanceX = posControl.actualState.vel.x * posControl.posDecelerationTime;
|
||||
const float stoppingDistanceY = posControl.actualState.vel.y * posControl.posDecelerationTime;
|
||||
const float stoppingDistanceX = navGetCurrentActualPositionAndVelocity()->vel.x * posControl.posDecelerationTime;
|
||||
const float stoppingDistanceY = navGetCurrentActualPositionAndVelocity()->vel.y * posControl.posDecelerationTime;
|
||||
|
||||
pos->x = posControl.actualState.pos.x + stoppingDistanceX;
|
||||
pos->y = posControl.actualState.pos.y + stoppingDistanceY;
|
||||
pos->x = navGetCurrentActualPositionAndVelocity()->pos.x + stoppingDistanceX;
|
||||
pos->y = navGetCurrentActualPositionAndVelocity()->pos.y + stoppingDistanceY;
|
||||
}
|
||||
|
||||
void resetMulticopterHeadingController(void)
|
||||
|
|
|
|||
|
|
@ -828,8 +828,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
posEstimator.est.epv = newEPV;
|
||||
|
||||
/* AGL estimation */
|
||||
#ifdef USE_RANGEFINDER
|
||||
if (isSurfaceValid) { // If surface topic is updated in timely manner - do something smart
|
||||
#if defined(USE_RANGEFINDER) && defined(USE_BARO)
|
||||
if (isSurfaceValid && isBaroValid) {
|
||||
navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual;
|
||||
bool resetSurfaceEstimate = false;
|
||||
switch (posEstimator.est.aglQual) {
|
||||
|
|
@ -893,7 +893,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
|
||||
// Correct estimate from rangefinder
|
||||
const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
|
||||
const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f);
|
||||
const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 75.0f), 0.0f, 1.0f, 0.1f, 1.0f);
|
||||
|
||||
posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * dt;
|
||||
posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * dt;
|
||||
|
|
|
|||
|
|
@ -71,9 +71,9 @@ typedef struct navigationFlags_s {
|
|||
bool verticalPositionDataConsumed;
|
||||
|
||||
navigationEstimateStatus_e estAltStatus; // Indicates that we have a working altitude sensor (got at least one valid reading from it)
|
||||
navigationEstimateStatus_e estPosStatue; // Indicates that GPS is working (or not)
|
||||
navigationEstimateStatus_e estPosStatus; // Indicates that GPS is working (or not)
|
||||
navigationEstimateStatus_e estAglStatus;
|
||||
navigationEstimateStatus_e estHeadingStatus; // Indicate valid heading - wither mag or GPS at certain speed on airplane
|
||||
navigationEstimateStatus_e estSurfaceStatus;
|
||||
|
||||
bool isAdjustingPosition;
|
||||
bool isAdjustingAltitude;
|
||||
|
|
@ -131,11 +131,17 @@ typedef struct navigationPIDControllers_s {
|
|||
typedef struct {
|
||||
fpVector3_t pos;
|
||||
fpVector3_t vel;
|
||||
} navEstimatedPosVel_t;
|
||||
|
||||
typedef struct {
|
||||
// Local estimated states
|
||||
navEstimatedPosVel_t abs;
|
||||
navEstimatedPosVel_t agl;
|
||||
int32_t yaw;
|
||||
|
||||
// Service values
|
||||
float sinYaw;
|
||||
float cosYaw;
|
||||
float surface;
|
||||
float surfaceVel;
|
||||
float surfaceMin;
|
||||
float velXY;
|
||||
} navigationEstimatedState_t;
|
||||
|
|
@ -144,7 +150,6 @@ typedef struct {
|
|||
fpVector3_t pos;
|
||||
fpVector3_t vel;
|
||||
int32_t yaw;
|
||||
float surface;
|
||||
} navigationDesiredState_t;
|
||||
|
||||
typedef enum {
|
||||
|
|
@ -156,7 +161,6 @@ typedef enum {
|
|||
|
||||
NAV_FSM_EVENT_SWITCH_TO_IDLE,
|
||||
NAV_FSM_EVENT_SWITCH_TO_ALTHOLD,
|
||||
NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D,
|
||||
NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D,
|
||||
NAV_FSM_EVENT_SWITCH_TO_RTH,
|
||||
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT,
|
||||
|
|
@ -180,9 +184,6 @@ typedef enum {
|
|||
NAV_STATE_ALTHOLD_INITIALIZE, // 2
|
||||
NAV_STATE_ALTHOLD_IN_PROGRESS, // 3
|
||||
|
||||
NAV_STATE_POSHOLD_2D_INITIALIZE, // 4
|
||||
NAV_STATE_POSHOLD_2D_IN_PROGRESS, // 5
|
||||
|
||||
NAV_STATE_POSHOLD_3D_INITIALIZE, // 6
|
||||
NAV_STATE_POSHOLD_3D_IN_PROGRESS, // 7
|
||||
|
||||
|
|
@ -302,6 +303,8 @@ typedef struct {
|
|||
extern navigationPosControl_t posControl;
|
||||
|
||||
/* Internally used functions */
|
||||
const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);
|
||||
|
||||
float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags);
|
||||
float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler);
|
||||
void navPidReset(pidController_t *pid);
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@
|
|||
* time to send frame: 3ms.
|
||||
*/
|
||||
|
||||
#define SBUS_MIN_INTERFRAME_DELAY_US 5000 // According to FrSky interframe is 6.67ms, we go smaller just in case
|
||||
#define SBUS_MIN_INTERFRAME_DELAY_US 3000 // According to FrSky interframe is 6.67ms, we go smaller just in case
|
||||
|
||||
#define SBUS_FRAME_SIZE (SBUS_CHANNEL_DATA_LENGTH + 2)
|
||||
|
||||
|
|
|
|||
|
|
@ -165,7 +165,11 @@
|
|||
#define USE_MAG_IST8308
|
||||
#define USE_MAG_MAG3110
|
||||
|
||||
#define USE_OPTICAL_FLOW
|
||||
#define USE_OPFLOW_MSP
|
||||
|
||||
#define USE_RANGEFINDER
|
||||
#define USE_RANGEFINDER_MSP
|
||||
#define USE_RANGEFINDER_HCSR04_I2C
|
||||
#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue