mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-19 14:25:16 +03:00
Merge branch 'development' into dzikuvx-nav-cruise-improvements
This commit is contained in:
Pawel Spychalski (DzikuVx)
commit
cb0f1e83aa
317 changed files with 5662 additions and 3252 deletions
|
|
@ -80,7 +80,7 @@ radar_pois_t radar_pois[RADAR_MAX_POIS];
|
|||
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
|
||||
#endif
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7);
|
||||
|
||||
PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
||||
.general = {
|
||||
|
|
@ -143,6 +143,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.max_dive_angle = 15, // degrees
|
||||
.cruise_throttle = 1400,
|
||||
.cruise_speed = 0, // cm/s
|
||||
.control_smoothness = 0,
|
||||
.max_throttle = 1700,
|
||||
.min_throttle = 1200,
|
||||
.pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle)
|
||||
|
|
@ -198,6 +199,10 @@ static void setupAltitudeController(void);
|
|||
static void resetHeadingController(void);
|
||||
void resetGCSFlags(void);
|
||||
|
||||
static void setupJumpCounters(void);
|
||||
static void resetJumpCounter(void);
|
||||
static void clearJumpCounters(void);
|
||||
|
||||
static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
|
||||
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
|
||||
void calculateInitialHoldPosition(fpVector3_t * pos);
|
||||
|
|
@ -232,6 +237,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
|
|||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState);
|
||||
|
|
@ -666,6 +672,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
|
|
@ -678,6 +685,27 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_WAYPOINT_HOLD_TIME] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME, // There is no state for timed hold?
|
||||
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_HOLD_TIMED,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOLD_TIME, // re-process the state
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, // 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_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,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_WAYPOINT_RTH_LAND] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
|
||||
|
|
@ -717,7 +745,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
|
||||
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_WP_ENROUTE,
|
||||
.mwError = MW_NAV_ERROR_FINISH,
|
||||
|
|
@ -1117,6 +1145,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
if (!STATE(ALTITUDE_CONTROL)) {
|
||||
//If altitude control is not a thing, switch to RTH in progress instead
|
||||
return NAV_FSM_EVENT_SUCCESS; //Will cause NAV_STATE_RTH_HEAD_HOME
|
||||
}
|
||||
|
||||
// If we have valid pos sensor OR we are configured to ignore GPS loss
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
|
||||
|
|
@ -1222,6 +1255,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
|
|||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
//On ROVER and BOAT we immediately switch to the next event
|
||||
if (!STATE(ALTITUDE_CONTROL)) {
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
// If position ok OR within valid timeout - continue
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
|
||||
|
||||
|
|
@ -1284,6 +1322,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
//On ROVER and BOAT we immediately switch to the next event
|
||||
if (!STATE(ALTITUDE_CONTROL)) {
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
|
@ -1327,7 +1370,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigatio
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (navConfig()->general.flags.disarm_on_landing) {
|
||||
//On ROVER and BOAT disarm immediately
|
||||
if (!STATE(ALTITUDE_CONTROL) || navConfig()->general.flags.disarm_on_landing) {
|
||||
disarm(DISARM_NAVIGATION);
|
||||
}
|
||||
|
||||
|
|
@ -1338,11 +1382,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
|
|||
{
|
||||
// Stay in this state
|
||||
UNUSED(previousState);
|
||||
updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL); // FIXME
|
||||
|
||||
if (STATE(ALTITUDE_CONTROL)) {
|
||||
updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL); // FIXME
|
||||
}
|
||||
|
||||
// Prevent I-terms growing when already landed
|
||||
pidResetErrorAccumulators();
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
|
|
@ -1360,7 +1406,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
|
|||
// Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
|
||||
resetAltitudeController(false);
|
||||
setupAltitudeController();
|
||||
|
||||
/*
|
||||
Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
|
||||
Using p3 minimises the risk of saving an invalid counter if a mission is aborted.
|
||||
*/
|
||||
setupJumpCounters();
|
||||
posControl.activeWaypointIndex = 0;
|
||||
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
|
||||
}
|
||||
|
|
@ -1372,15 +1422,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
|
|||
UNUSED(previousState);
|
||||
|
||||
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
|
||||
case NAV_WP_ACTION_HOLD_TIME:
|
||||
case NAV_WP_ACTION_WAYPOINT:
|
||||
case NAV_WP_ACTION_LAND:
|
||||
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
|
||||
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
|
||||
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
|
||||
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
|
||||
|
||||
// We use p3 as the volatile jump counter (p2 is the static value)
|
||||
case NAV_WP_ACTION_JUMP:
|
||||
if(posControl.waypointList[posControl.activeWaypointIndex].p2 != -1){
|
||||
if(posControl.waypointList[posControl.activeWaypointIndex].p2 == 0){
|
||||
if(posControl.waypointList[posControl.activeWaypointIndex].p3 != -1){
|
||||
if(posControl.waypointList[posControl.activeWaypointIndex].p3 == 0){
|
||||
resetJumpCounter();
|
||||
const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
|
||||
(posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
|
||||
|
||||
|
|
@ -1395,11 +1449,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
|
|||
}
|
||||
else
|
||||
{
|
||||
posControl.waypointList[posControl.activeWaypointIndex].p2--;
|
||||
posControl.waypointList[posControl.activeWaypointIndex].p3--;
|
||||
}
|
||||
}
|
||||
|
||||
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 - 1;
|
||||
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1;
|
||||
|
||||
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
|
||||
|
||||
|
|
@ -1420,7 +1474,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
|
|||
// If no position sensor available - land immediately
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
|
||||
case NAV_WP_ACTION_HOLD_TIME:
|
||||
case NAV_WP_ACTION_WAYPOINT:
|
||||
case NAV_WP_ACTION_LAND:
|
||||
if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
|
||||
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
|
||||
}
|
||||
|
|
@ -1444,7 +1500,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
|
|||
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
|
||||
}
|
||||
else {
|
||||
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
|
||||
if(navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY))
|
||||
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
|
||||
else
|
||||
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
|
||||
setDesiredPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL), 0, NAV_POS_UPDATE_Z);
|
||||
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
|
||||
}
|
||||
|
|
@ -1480,11 +1539,34 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
|
|||
else {
|
||||
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
|
||||
}
|
||||
|
||||
case NAV_WP_ACTION_LAND:
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
|
||||
|
||||
case NAV_WP_ACTION_HOLD_TIME:
|
||||
// Save the current time for the time the waypoint was reached
|
||||
posControl.wpReachedTime = millis();
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME;
|
||||
}
|
||||
|
||||
UNREACHABLE();
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
timeMs_t currentTime = millis();
|
||||
|
||||
if(posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0)
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
|
||||
if(posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
|
||||
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
|
@ -1522,6 +1604,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
clearJumpCounters();
|
||||
// If no position sensor available - land immediately
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
|
|
@ -1587,7 +1670,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
|
|||
|
||||
//allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
|
||||
if (feature(FEATURE_FW_LAUNCH)) {
|
||||
throttleStatus_e throttleStatus = calculateThrottleStatus();
|
||||
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
|
||||
if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())) {
|
||||
abortFixedWingLaunch();
|
||||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
|
|
@ -1825,11 +1908,6 @@ float navPidApply3(
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Limit both output and Iterm to limit windup
|
||||
*/
|
||||
pid->integrator = constrain(pid->integrator, outMin, outMax);
|
||||
|
||||
return outValConstrained;
|
||||
}
|
||||
|
||||
|
|
@ -2512,6 +2590,36 @@ static bool adjustAltitudeFromRCInput(void)
|
|||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Jump Counter support functions
|
||||
*-----------------------------------------------------------*/
|
||||
static void setupJumpCounters(void)
|
||||
{
|
||||
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
|
||||
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
|
||||
posControl.waypointList[wp].p3 = posControl.waypointList[wp].p2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void resetJumpCounter(void)
|
||||
{
|
||||
// reset the volatile counter from the set / static value
|
||||
posControl.waypointList[posControl.activeWaypointIndex].p3 =
|
||||
posControl.waypointList[posControl.activeWaypointIndex].p2;
|
||||
}
|
||||
|
||||
static void clearJumpCounters(void)
|
||||
{
|
||||
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
|
||||
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP) {
|
||||
posControl.waypointList[wp].p3 = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Heading controller (pass-through to MAG mode)
|
||||
*-----------------------------------------------------------*/
|
||||
|
|
@ -2606,10 +2714,14 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
|
|||
wpData->lon = wpLLH.lon;
|
||||
wpData->alt = wpLLH.alt;
|
||||
}
|
||||
// WP #1 - #15 - common waypoints - pre-programmed mission
|
||||
// WP #1 - #60 - common waypoints - pre-programmed mission
|
||||
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
|
||||
if (wpNumber <= posControl.waypointCount) {
|
||||
*wpData = posControl.waypointList[wpNumber - 1];
|
||||
if(wpData->action == NAV_WP_ACTION_JUMP) {
|
||||
wpData->p1 += 1; // make WP # (vice index)
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -2653,10 +2765,13 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
|
|||
}
|
||||
// WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
|
||||
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
|
||||
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH) {
|
||||
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND) {
|
||||
// Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
|
||||
if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
|
||||
posControl.waypointList[wpNumber - 1] = *wpData;
|
||||
if(wpData->action == NAV_WP_ACTION_JUMP) {
|
||||
posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #)
|
||||
}
|
||||
posControl.waypointCount = wpNumber;
|
||||
posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
|
||||
}
|
||||
|
|
@ -2780,6 +2895,11 @@ bool isApproachingLastWaypoint(void)
|
|||
}
|
||||
}
|
||||
|
||||
bool isWaypointWait(void)
|
||||
{
|
||||
return NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
|
||||
}
|
||||
|
||||
float getActiveWaypointSpeed(void)
|
||||
{
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
|
|
@ -2790,8 +2910,13 @@ float getActiveWaypointSpeed(void)
|
|||
uint16_t 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) {
|
||||
const float wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
|
||||
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;
|
||||
}
|
||||
|
|
@ -2873,7 +2998,9 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
|
||||
/* Process controllers */
|
||||
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
if (STATE(ROVER) || STATE(BOAT)) {
|
||||
applyRoverBoatNavigationController(navStateFlags, currentTimeUs);
|
||||
} else if (STATE(FIXED_WING_LEGACY)) {
|
||||
applyFixedWingNavigationController(navStateFlags, currentTimeUs);
|
||||
}
|
||||
else {
|
||||
|
|
@ -2960,7 +3087,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
else {
|
||||
// If we were in LAUNCH mode - force switch to IDLE only if the throttle is low
|
||||
if (FLIGHT_MODE(NAV_LAUNCH_MODE)) {
|
||||
throttleStatus_e throttleStatus = calculateThrottleStatus();
|
||||
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
|
||||
if (throttleStatus != THROTTLE_LOW)
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
else
|
||||
|
|
@ -3127,16 +3254,27 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
|
|||
}
|
||||
}
|
||||
|
||||
// Don't allow arming if any of JUMP waypoint has invalid settings
|
||||
/*
|
||||
* Don't allow arming if any of JUMP waypoint has invalid settings
|
||||
* First WP can't be JUMP
|
||||
* Can't jump to immediately adjacent WPs (pointless)
|
||||
* Can't jump beyond WP list
|
||||
* Only jump to geo-referenced WP types
|
||||
*/
|
||||
if (posControl.waypointCount > 0) {
|
||||
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
|
||||
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
|
||||
if((posControl.waypointList[wp].p1 > posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)){
|
||||
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
|
||||
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
|
||||
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
|
||||
if((wp == 0) || ((posControl.waypointList[wp].p1 > (wp-2)) && (posControl.waypointList[wp].p1 < (wp+2)) ) || (posControl.waypointList[wp].p1 >= posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)) {
|
||||
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
|
||||
}
|
||||
/* check for target geo-ref sanity */
|
||||
uint16_t target = posControl.waypointList[wp].p1;
|
||||
if(!(posControl.waypointList[target].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[target].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[target].action == NAV_WP_ACTION_LAND)) {
|
||||
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return NAV_ARMING_BLOCKER_NONE;
|
||||
}
|
||||
|
|
@ -3280,7 +3418,7 @@ void navigationUsePIDs(void)
|
|||
0.0f,
|
||||
NAV_DTERM_CUT_HZ
|
||||
);
|
||||
|
||||
|
||||
navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f,
|
||||
(float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f,
|
||||
(float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f,
|
||||
|
|
@ -3395,12 +3533,15 @@ bool navigationIsFlyingAutonomousMode(void)
|
|||
|
||||
bool navigationRTHAllowsLanding(void)
|
||||
{
|
||||
if (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)
|
||||
return true;
|
||||
|
||||
navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
|
||||
return allow == NAV_RTH_ALLOW_LANDING_ALWAYS ||
|
||||
(allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE));
|
||||
}
|
||||
|
||||
bool FAST_CODE isNavLaunchEnabled(void)
|
||||
bool isNavLaunchEnabled(void)
|
||||
{
|
||||
return IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH);
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue