Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 00:05:28 +03:00
Code Issues Wiki Activity

Merge branch 'development' into dzikuvx-correct-3d-throttle-handling

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-03-07 13:22:21 +01:00
commit fab7de6a48
71 changed files with 1065 additions and 2468 deletions
Download patch file Download diff file Expand all files Collapse all files

77
src/main/navigation/navigation.c
View file

@ -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, 5);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -165,7 +165,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_climb_angle = 18, // 18 degrees
.launch_max_angle = 45, // 45 deg
.cruise_yaw_rate = 20, // 20dps
.allow_manual_thr_increase = false
.allow_manual_thr_increase = false,
.useFwNavYawControl = 0,
.yawControlDeadband = 0,
}
);
@ -617,12 +619,13 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_STATE_WAYPOINT_PRE_ACTION] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION,
.timeoutMs = 0,
.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_PROCESS_NEXT,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP)
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
@ -1374,6 +1377,31 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
case NAV_WP_ACTION_JUMP:
if(posControl.waypointList[posControl.activeWaypointIndex].p2 != -1){
if(posControl.waypointList[posControl.activeWaypointIndex].p2 == 0){
const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
(posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
if (isLastWaypoint) {
// JUMP is the last waypoint and we reached the last jump, switch to finish.
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} else {
// Finished JUMP, move to next WP
posControl.activeWaypointIndex++;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
}
}
else
{
posControl.waypointList[posControl.activeWaypointIndex].p2--;
}
}
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 - 1;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
case NAV_WP_ACTION_RTH:
posControl.rthState.rthInitialDistance = posControl.homeDistance;
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
@ -1407,6 +1435,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
}
break;
case NAV_WP_ACTION_JUMP:
UNREACHABLE();
case NAV_WP_ACTION_RTH:
if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
@ -1438,6 +1469,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
case NAV_WP_ACTION_WAYPOINT:
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
case NAV_WP_ACTION_JUMP:
UNREACHABLE();
case NAV_WP_ACTION_RTH:
if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) {
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
@ -1779,6 +1813,11 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
}
}
/*
* Limit both output and Iterm to limit windup
*/
pid->integrator = constrain(pid->integrator, outMin, outMax);
return outValConstrained;
}
@ -2600,9 +2639,9 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
setDesiredPosition(&wpPos.pos, DEGREES_TO_CENTIDEGREES(wpData->p1), waypointUpdateFlags);
}
// WP #1 - #15 - common waypoints - pre-programmed mission
// 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_RTH) {
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH) {
// 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;
@ -3076,6 +3115,17 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
}
}
// Don't allow arming if any of JUMP waypoint has invalid settings
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;
}
}
}
}
return NAV_ARMING_BLOCKER_NONE;
}
@ -3210,6 +3260,13 @@ 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,
0.0f,
2.0f
);
}
void navigationInit(void)
@ -3240,6 +3297,16 @@ void navigationInit(void)
/* Use system config */
navigationUsePIDs();
if (
mixerConfig()->platformType == PLATFORM_BOAT ||
mixerConfig()->platformType == PLATFORM_ROVER ||
navConfig()->fw.useFwNavYawControl
) {
ENABLE_STATE(FW_HEADING_USE_YAW);
} else {
DISABLE_STATE(FW_HEADING_USE_YAW);
}
}
/*-----------------------------------------------------------