mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 01:35:35 +03:00
Merge branch 'master' of https://github.com/RomanLut/inav into submit-gps-fix-estimation
removed GPS Off box implemented Disable GPS sensor fix logic condition
This commit is contained in:
Roman Lut
commit
cb2d448911
379 changed files with 5433 additions and 3656 deletions
|
|
@ -40,6 +40,7 @@
|
|||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
#ifdef USE_MULTI_MISSION
|
||||
#include "fc/rc_adjustments.h"
|
||||
#include "fc/cli.h"
|
||||
#endif
|
||||
#include "fc/settings.h"
|
||||
|
|
@ -98,7 +99,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
|
|||
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2);
|
||||
#endif
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 2);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 3);
|
||||
|
||||
PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
||||
.general = {
|
||||
|
|
@ -126,7 +127,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
// General navigation parameters
|
||||
.pos_failure_timeout = SETTING_NAV_POSITION_TIMEOUT_DEFAULT, // 5 sec
|
||||
.waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT, // 2m diameter
|
||||
.waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT, // centimeters - first waypoint should be closer than this
|
||||
.waypoint_safe_distance = SETTING_NAV_WP_MAX_SAFE_DISTANCE_DEFAULT, // Metres - first waypoint should be closer than this
|
||||
#ifdef USE_MULTI_MISSION
|
||||
.waypoint_multi_mission_index = SETTING_NAV_WP_MULTI_MISSION_INDEX_DEFAULT, // mission index selected from multi mission WP entry
|
||||
#endif
|
||||
|
|
@ -151,12 +152,13 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.max_altitude = SETTING_NAV_MAX_ALTITUDE_DEFAULT,
|
||||
.rth_trackback_distance = SETTING_NAV_RTH_TRACKBACK_DISTANCE_DEFAULT, // Max distance allowed for RTH trackback
|
||||
.waypoint_enforce_altitude = SETTING_NAV_WP_ENFORCE_ALTITUDE_DEFAULT, // Forces set wp altitude to be achieved
|
||||
.land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT, // Changes sensitivity of landing detection
|
||||
.auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled
|
||||
},
|
||||
|
||||
// MC-specific
|
||||
.mc = {
|
||||
.max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees
|
||||
.auto_disarm_delay = SETTING_NAV_MC_AUTO_DISARM_DELAY_DEFAULT, // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed
|
||||
|
||||
#ifdef USE_MR_BRAKING_MODE
|
||||
.braking_speed_threshold = SETTING_NAV_MC_BRAKING_SPEED_THRESHOLD_DEFAULT, // Braking can become active above 1m/s
|
||||
|
|
@ -183,7 +185,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
|
||||
.pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
|
||||
.pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
|
||||
.minThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT,
|
||||
.loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m
|
||||
.loiter_direction = SETTING_FW_LOITER_DIRECTION_DEFAULT,
|
||||
|
||||
//Fixed wing landing
|
||||
.land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT, // 2 degrees dive by default
|
||||
|
|
@ -203,21 +207,23 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg
|
||||
.launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF
|
||||
.launch_abort_deadband = SETTING_NAV_FW_LAUNCH_ABORT_DEADBAND_DEFAULT, // 100 us
|
||||
|
||||
.cruise_yaw_rate = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT, // 20dps
|
||||
.allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
|
||||
.useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
|
||||
.yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
|
||||
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled
|
||||
.auto_disarm_delay = SETTING_NAV_FW_AUTO_DISARM_DELAY_DEFAULT, // ms - time delay to disarm when auto disarm after landing enabled
|
||||
.wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions
|
||||
.wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs CR67
|
||||
.wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs
|
||||
.wp_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // 0, smooths turns during FW WP mode missions
|
||||
}
|
||||
);
|
||||
|
||||
/* NAV variables */
|
||||
static navWapointHeading_t wpHeadingControl;
|
||||
navigationPosControl_t posControl;
|
||||
navSystemStatus_t NAV_Status;
|
||||
navigationPosControl_t posControl;
|
||||
navSystemStatus_t NAV_Status;
|
||||
|
||||
EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
|
||||
|
||||
// Blackbox states
|
||||
|
|
@ -657,7 +663,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_RTH_FINISHING,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
|
||||
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
|
||||
.mwError = MW_NAV_ERROR_LANDING,
|
||||
|
|
@ -671,7 +677,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_RTH_FINISHED,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHED,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
|
||||
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_LANDED,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
|
|
@ -1462,6 +1468,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
}
|
||||
|
||||
float descentVelLimited = 0;
|
||||
int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z;
|
||||
|
||||
// A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
|
||||
if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
|
||||
|
|
@ -1471,8 +1478,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
} else {
|
||||
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
|
||||
float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z,
|
||||
navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt,
|
||||
navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
|
||||
navConfig()->general.land_slowdown_minalt + landingElevation,
|
||||
navConfig()->general.land_slowdown_maxalt + landingElevation,
|
||||
navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
|
||||
|
||||
descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
|
||||
}
|
||||
|
|
@ -1526,14 +1534,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
|
|||
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.
|
||||
*/
|
||||
if (posControl.activeWaypointIndex == 0 || posControl.wpMissionRestart) {
|
||||
if (posControl.activeWaypointIndex == posControl.startWpIndex || posControl.wpMissionRestart) {
|
||||
setupJumpCounters();
|
||||
posControl.activeWaypointIndex = 0;
|
||||
posControl.activeWaypointIndex = posControl.startWpIndex;
|
||||
wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
|
||||
}
|
||||
|
||||
if (navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_SWITCH) {
|
||||
posControl.wpMissionRestart = posControl.activeWaypointIndex ? !posControl.wpMissionRestart : false;
|
||||
posControl.wpMissionRestart = posControl.activeWaypointIndex > posControl.startWpIndex ? !posControl.wpMissionRestart : false;
|
||||
} else {
|
||||
posControl.wpMissionRestart = navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_START;
|
||||
}
|
||||
|
|
@ -1544,14 +1552,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
|
|||
|
||||
static navigationFSMEvent_t nextForNonGeoStates(void)
|
||||
{
|
||||
/* simple helper for non-geographical states that just set other data */
|
||||
const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) || (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
|
||||
|
||||
if (isLastWaypoint) {
|
||||
// non-geo state is the last waypoint, switch to finish.
|
||||
/* simple helper for non-geographical states that just set other data */
|
||||
if (isLastMissionWaypoint()) { // non-geo state is the last waypoint, switch to finish.
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
|
||||
} else {
|
||||
// Finished non-geo, move to next WP
|
||||
} else { // Finished non-geo, move to next WP
|
||||
posControl.activeWaypointIndex++;
|
||||
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
|
||||
}
|
||||
|
|
@ -1574,8 +1578,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
|
|||
|
||||
// We use p3 as the volatile jump counter (p2 is the static value)
|
||||
case NAV_WP_ACTION_JUMP:
|
||||
if(posControl.waypointList[posControl.activeWaypointIndex].p3 != -1){
|
||||
if(posControl.waypointList[posControl.activeWaypointIndex].p3 == 0){
|
||||
if (posControl.waypointList[posControl.activeWaypointIndex].p3 != -1) {
|
||||
if (posControl.waypointList[posControl.activeWaypointIndex].p3 == 0) {
|
||||
resetJumpCounter();
|
||||
return nextForNonGeoStates();
|
||||
}
|
||||
|
|
@ -1584,7 +1588,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
|
|||
posControl.waypointList[posControl.activeWaypointIndex].p3--;
|
||||
}
|
||||
}
|
||||
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1;
|
||||
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 + posControl.startWpIndex;
|
||||
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
|
||||
|
||||
case NAV_WP_ACTION_SET_POI:
|
||||
|
|
@ -1809,8 +1813,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
rcCommand[THROTTLE] = getThrottleIdleValue();
|
||||
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
|
||||
disarm(DISARM_NAVIGATION);
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
|
@ -1931,7 +1934,7 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
|
|||
if (posControl.flags.isAdjustingPosition) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_POSITION;
|
||||
if (posControl.flags.isAdjustingAltitude) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_ALTITUDE;
|
||||
|
||||
NAV_Status.activeWpNumber = posControl.activeWaypointIndex + 1;
|
||||
NAV_Status.activeWpNumber = posControl.activeWaypointIndex - posControl.startWpIndex + 1;
|
||||
NAV_Status.activeWpAction = 0;
|
||||
if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) {
|
||||
NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action;
|
||||
|
|
@ -2116,7 +2119,7 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
|
|||
/*-----------------------------------------------------------
|
||||
* Processes an update to estimated heading
|
||||
*-----------------------------------------------------------*/
|
||||
void updateActualHeading(bool headingValid, int32_t newHeading)
|
||||
void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse)
|
||||
{
|
||||
/* Update heading. Check if we're acquiring a valid heading for the
|
||||
* first time and update home heading accordingly.
|
||||
|
|
@ -2147,7 +2150,9 @@ void updateActualHeading(bool headingValid, int32_t newHeading)
|
|||
}
|
||||
posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
|
||||
}
|
||||
/* Use course over ground for fixed wing nav "heading" when valid - TODO use heading and cog as specifically required for FW and MR */
|
||||
posControl.actualState.yaw = newHeading;
|
||||
posControl.actualState.cog = newGroundCourse; // currently only used for OSD display
|
||||
posControl.flags.estHeadingStatus = newEstHeading;
|
||||
|
||||
/* Precompute sin/cos of yaw angle */
|
||||
|
|
@ -2222,7 +2227,7 @@ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3
|
|||
static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
|
||||
{
|
||||
// Only for WP Mode not Trackback. Ignore non geo waypoints except RTH and JUMP.
|
||||
if (FLIGHT_MODE(NAV_WP_MODE) && !isLastMissionWaypoint()) {
|
||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP && !isLastMissionWaypoint()) {
|
||||
navWaypointActions_e nextWpAction = posControl.waypointList[posControl.activeWaypointIndex + 1].action;
|
||||
|
||||
if (!(nextWpAction == NAV_WP_ACTION_SET_POI || nextWpAction == NAV_WP_ACTION_SET_HEAD)) {
|
||||
|
|
@ -2230,8 +2235,8 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
|
|||
if (nextWpAction == NAV_WP_ACTION_JUMP) {
|
||||
if (posControl.waypointList[posControl.activeWaypointIndex + 1].p3 != 0 ||
|
||||
posControl.waypointList[posControl.activeWaypointIndex + 1].p2 == -1) {
|
||||
nextWpIndex = posControl.waypointList[posControl.activeWaypointIndex + 1].p1;
|
||||
} else if (posControl.activeWaypointIndex + 2 <= posControl.waypointCount - 1) {
|
||||
nextWpIndex = posControl.waypointList[posControl.activeWaypointIndex + 1].p1 + posControl.startWpIndex;
|
||||
} else if (posControl.activeWaypointIndex + 2 <= posControl.startWpIndex + posControl.waypointCount - 1) {
|
||||
if (posControl.waypointList[posControl.activeWaypointIndex + 2].action != NAV_WP_ACTION_JUMP) {
|
||||
nextWpIndex++;
|
||||
} else {
|
||||
|
|
@ -2262,6 +2267,11 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w
|
|||
}
|
||||
|
||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) {
|
||||
// If WP turn smoothing CUT option used WP is reached when start of turn is initiated
|
||||
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) {
|
||||
posControl.flags.wpTurnSmoothingActive = false;
|
||||
return true;
|
||||
}
|
||||
// Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw
|
||||
// Same method for turn smoothing option but relative bearing set at 60 degrees
|
||||
uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000;
|
||||
|
|
@ -2648,10 +2658,17 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
|
|||
* Reverts to normal RTH heading direct to home when end of track reached.
|
||||
* Trackpoints logged with precedence for course/altitude changes. Distance based changes
|
||||
* only logged if no course/altitude changes logged over an extended distance.
|
||||
* Tracking suspended during fixed wing loiter (PosHold and WP Mode timed hold).
|
||||
* --------------------------------------------------------------------------------- */
|
||||
static void updateRthTrackback(bool forceSaveTrackPoint)
|
||||
{
|
||||
if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED)) {
|
||||
static bool suspendTracking = false;
|
||||
bool fwLoiterIsActive = STATE(AIRPLANE) && (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED || FLIGHT_MODE(NAV_POSHOLD_MODE));
|
||||
if (!fwLoiterIsActive && suspendTracking) {
|
||||
suspendTracking = false;
|
||||
}
|
||||
|
||||
if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED || suspendTracking)) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -2700,6 +2717,11 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
|
|||
saveTrackpoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]) > METERS_TO_CENTIMETERS(20);
|
||||
previousTBTripDist = posControl.totalTripDistance;
|
||||
}
|
||||
|
||||
// Suspend tracking during loiter on fixed wing. Save trackpoint at start of loiter.
|
||||
if (distanceCounter && fwLoiterIsActive) {
|
||||
saveTrackpoint = suspendTracking = true;
|
||||
}
|
||||
}
|
||||
|
||||
// when trackpoint store full, overwrite from start of store using 'rthTBWrapAroundCounter' to track overwrite position
|
||||
|
|
@ -2899,9 +2921,16 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
|
|||
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target
|
||||
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
|
||||
|
||||
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
|
||||
posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
|
||||
posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500); // FIXME: calculate sanity limits in a smarter way
|
||||
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) {
|
||||
static bool targetHoldActive = false;
|
||||
// Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target
|
||||
if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) {
|
||||
posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
|
||||
targetHoldActive = false;
|
||||
} else if (!targetHoldActive) { // Reset and hold target to actual + climb rate boost until actual catches up
|
||||
posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate;
|
||||
targetHoldActive = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
|
|
@ -2951,7 +2980,7 @@ static bool adjustAltitudeFromRCInput(void)
|
|||
*-----------------------------------------------------------*/
|
||||
static void setupJumpCounters(void)
|
||||
{
|
||||
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
|
||||
for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; wp++) {
|
||||
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
|
||||
posControl.waypointList[wp].p3 = posControl.waypointList[wp].p2;
|
||||
}
|
||||
|
|
@ -2961,13 +2990,12 @@ static void setupJumpCounters(void)
|
|||
static void resetJumpCounter(void)
|
||||
{
|
||||
// reset the volatile counter from the set / static value
|
||||
posControl.waypointList[posControl.activeWaypointIndex].p3 =
|
||||
posControl.waypointList[posControl.activeWaypointIndex].p2;
|
||||
posControl.waypointList[posControl.activeWaypointIndex].p3 = posControl.waypointList[posControl.activeWaypointIndex].p2;
|
||||
}
|
||||
|
||||
static void clearJumpCounters(void)
|
||||
{
|
||||
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
|
||||
for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; wp++) {
|
||||
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP) {
|
||||
posControl.waypointList[wp].p3 = 0;
|
||||
}
|
||||
|
|
@ -3086,12 +3114,11 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
|
|||
}
|
||||
// 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 (wpNumber <= getWaypointCount()) {
|
||||
*wpData = posControl.waypointList[wpNumber - 1 + (ARMING_FLAG(ARMED) ? posControl.startWpIndex : 0)];
|
||||
if(wpData->action == NAV_WP_ACTION_JUMP) {
|
||||
wpData->p1 += 1; // make WP # (vice index)
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -3164,17 +3191,15 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
|
|||
|
||||
void resetWaypointList(void)
|
||||
{
|
||||
/* Can only reset waypoint list if not armed */
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
posControl.waypointCount = 0;
|
||||
posControl.waypointListValid = false;
|
||||
posControl.geoWaypointCount = 0;
|
||||
posControl.waypointCount = 0;
|
||||
posControl.waypointListValid = false;
|
||||
posControl.geoWaypointCount = 0;
|
||||
posControl.startWpIndex = 0;
|
||||
#ifdef USE_MULTI_MISSION
|
||||
posControl.loadedMultiMissionIndex = 0;
|
||||
posControl.loadedMultiMissionStartWP = 0;
|
||||
posControl.loadedMultiMissionWPCount = 0;
|
||||
posControl.totalMultiMissionWpCount = 0;
|
||||
posControl.loadedMultiMissionIndex = 0;
|
||||
posControl.multiMissionCount = 0;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
bool isWaypointListValid(void)
|
||||
|
|
@ -3184,31 +3209,85 @@ bool isWaypointListValid(void)
|
|||
|
||||
int getWaypointCount(void)
|
||||
{
|
||||
return posControl.waypointCount;
|
||||
uint8_t waypointCount = posControl.waypointCount;
|
||||
#ifdef USE_MULTI_MISSION
|
||||
if (!ARMING_FLAG(ARMED) && posControl.totalMultiMissionWpCount) {
|
||||
waypointCount = posControl.totalMultiMissionWpCount;
|
||||
}
|
||||
#endif
|
||||
return waypointCount;
|
||||
}
|
||||
|
||||
#ifdef USE_MULTI_MISSION
|
||||
void selectMultiMissionIndex(int8_t increment)
|
||||
{
|
||||
if (posControl.multiMissionCount > 1) { // stick selection only active when multi mission loaded
|
||||
navConfigMutable()->general.waypoint_multi_mission_index = constrain(navConfigMutable()->general.waypoint_multi_mission_index + increment, 0, posControl.multiMissionCount);
|
||||
navConfigMutable()->general.waypoint_multi_mission_index = constrain(navConfigMutable()->general.waypoint_multi_mission_index + increment, 1, posControl.multiMissionCount);
|
||||
}
|
||||
}
|
||||
|
||||
void setMultiMissionOnArm(void)
|
||||
void loadSelectedMultiMission(uint8_t missionIndex)
|
||||
{
|
||||
if (posControl.multiMissionCount > 1 && posControl.loadedMultiMissionWPCount) {
|
||||
posControl.waypointCount = posControl.loadedMultiMissionWPCount;
|
||||
uint8_t missionCount = 1;
|
||||
posControl.waypointCount = 0;
|
||||
posControl.geoWaypointCount = 0;
|
||||
|
||||
for (int8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) {
|
||||
posControl.waypointList[i] = posControl.waypointList[i + posControl.loadedMultiMissionStartWP];
|
||||
if (i == posControl.waypointCount - 1) {
|
||||
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
|
||||
if ((missionCount == missionIndex)) {
|
||||
/* store details of selected mission: start wp index, mission wp count, geo wp count */
|
||||
if (!(posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI ||
|
||||
posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD ||
|
||||
posControl.waypointList[i].action == NAV_WP_ACTION_JUMP)) {
|
||||
posControl.geoWaypointCount++;
|
||||
}
|
||||
// mission start WP
|
||||
if (posControl.waypointCount == 0) {
|
||||
posControl.waypointCount = 1; // start marker only, value unimportant (but not 0)
|
||||
posControl.startWpIndex = i;
|
||||
}
|
||||
// mission end WP
|
||||
if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
|
||||
posControl.waypointCount = i - posControl.startWpIndex + 1;
|
||||
break;
|
||||
}
|
||||
} else if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
|
||||
missionCount++;
|
||||
}
|
||||
|
||||
posControl.loadedMultiMissionStartWP = 0;
|
||||
posControl.loadedMultiMissionWPCount = 0;
|
||||
}
|
||||
|
||||
posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? missionIndex : 0;
|
||||
posControl.activeWaypointIndex = posControl.startWpIndex;
|
||||
}
|
||||
|
||||
bool updateWpMissionChange(void)
|
||||
{
|
||||
/* Function only called when ARMED */
|
||||
|
||||
if (posControl.multiMissionCount < 2 || posControl.wpPlannerActiveWPIndex || FLIGHT_MODE(NAV_WP_MODE)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
uint8_t setMissionIndex = navConfig()->general.waypoint_multi_mission_index;
|
||||
if (!(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION) || isAdjustmentFunctionSelected(ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX))) {
|
||||
/* reload mission if mission index changed */
|
||||
if (posControl.loadedMultiMissionIndex != setMissionIndex) {
|
||||
loadSelectedMultiMission(setMissionIndex);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool toggleFlag = false;
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && toggleFlag) {
|
||||
if (setMissionIndex == posControl.multiMissionCount) {
|
||||
navConfigMutable()->general.waypoint_multi_mission_index = 1;
|
||||
} else {
|
||||
selectMultiMissionIndex(1);
|
||||
}
|
||||
toggleFlag = false;
|
||||
} else if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) {
|
||||
toggleFlag = true;
|
||||
}
|
||||
return false; // block WP mode while changing mission when armed
|
||||
}
|
||||
|
||||
bool checkMissionCount(int8_t waypoint)
|
||||
|
|
@ -3227,13 +3306,8 @@ bool checkMissionCount(int8_t waypoint)
|
|||
#ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE
|
||||
bool loadNonVolatileWaypointList(bool clearIfLoaded)
|
||||
{
|
||||
#ifdef USE_MULTI_MISSION
|
||||
/* multi_mission_index 0 only used for non NVM missions - don't load.
|
||||
* Don't load if mission planner WP count > 0 */
|
||||
if (ARMING_FLAG(ARMED) || !navConfig()->general.waypoint_multi_mission_index || posControl.wpPlannerActiveWPIndex) {
|
||||
#else
|
||||
/* Don't load if armed or mission planner active */
|
||||
if (ARMING_FLAG(ARMED) || posControl.wpPlannerActiveWPIndex) {
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -3247,46 +3321,28 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded)
|
|||
if (navConfig()->general.waypoint_multi_mission_index > posControl.multiMissionCount) {
|
||||
navConfigMutable()->general.waypoint_multi_mission_index = 1;
|
||||
}
|
||||
posControl.multiMissionCount = 0;
|
||||
posControl.loadedMultiMissionWPCount = 0;
|
||||
int8_t loadedMultiMissionGeoWPCount;
|
||||
#endif
|
||||
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
|
||||
setWaypoint(i + 1, nonVolatileWaypointList(i));
|
||||
#ifdef USE_MULTI_MISSION
|
||||
/* store details of selected mission */
|
||||
if ((posControl.multiMissionCount + 1 == navConfig()->general.waypoint_multi_mission_index)) {
|
||||
// mission start WP
|
||||
if (posControl.loadedMultiMissionWPCount == 0) {
|
||||
posControl.loadedMultiMissionWPCount = 1; // start marker only, value here unimportant (but not 0)
|
||||
posControl.loadedMultiMissionStartWP = i;
|
||||
loadedMultiMissionGeoWPCount = posControl.geoWaypointCount;
|
||||
}
|
||||
// mission end WP
|
||||
if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
|
||||
posControl.loadedMultiMissionWPCount = i - posControl.loadedMultiMissionStartWP + 1;
|
||||
loadedMultiMissionGeoWPCount = posControl.geoWaypointCount - loadedMultiMissionGeoWPCount + 1;
|
||||
}
|
||||
}
|
||||
|
||||
/* count up number of missions */
|
||||
/* count up number of missions and exit after last multi mission */
|
||||
if (checkMissionCount(i)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
posControl.geoWaypointCount = loadedMultiMissionGeoWPCount;
|
||||
posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? navConfig()->general.waypoint_multi_mission_index : 0;
|
||||
posControl.totalMultiMissionWpCount = posControl.waypointCount;
|
||||
loadSelectedMultiMission(navConfig()->general.waypoint_multi_mission_index);
|
||||
|
||||
/* Mission sanity check failed - reset the list
|
||||
* Also reset if no selected mission loaded (shouldn't happen) */
|
||||
if (!posControl.waypointListValid || !posControl.loadedMultiMissionWPCount) {
|
||||
if (!posControl.waypointListValid || !posControl.waypointCount) {
|
||||
#else
|
||||
// check this is the last waypoint
|
||||
if (nonVolatileWaypointList(i)->flag == NAV_WP_FLAG_LAST) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Mission sanity check failed - reset the list
|
||||
if (!posControl.waypointListValid) {
|
||||
#endif
|
||||
|
|
@ -3357,7 +3413,7 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos
|
|||
|
||||
geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag)
|
||||
{
|
||||
return datumFlag == NAV_WP_MSL_DATUM ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE;
|
||||
return ((datumFlag & NAV_WP_MSL_DATUM) == NAV_WP_MSL_DATUM) ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE;
|
||||
}
|
||||
|
||||
static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
|
||||
|
|
@ -3378,7 +3434,7 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
|
|||
/* Checks if active waypoint is last in mission */
|
||||
bool isLastMissionWaypoint(void)
|
||||
{
|
||||
return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (posControl.waypointCount - 1) ||
|
||||
return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (posControl.startWpIndex + posControl.waypointCount - 1) ||
|
||||
(posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST));
|
||||
}
|
||||
|
||||
|
|
@ -3490,7 +3546,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
posControl.flags.forcedRTHActivated = false;
|
||||
posControl.flags.forcedEmergLandingActivated = false;
|
||||
// ensure WP missions always restart from first waypoint after disarm
|
||||
posControl.activeWaypointIndex = 0;
|
||||
posControl.activeWaypointIndex = posControl.startWpIndex;
|
||||
// Reset RTH trackback
|
||||
posControl.activeRthTBPointIndex = -1;
|
||||
posControl.flags.rthTrackbackActive = false;
|
||||
|
|
@ -3680,7 +3736,12 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
|
||||
// Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
|
||||
// Block activation if using WP Mission Planner
|
||||
// Also check multimission mission change status before activating WP mode
|
||||
#ifdef USE_MULTI_MISSION
|
||||
if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
|
||||
#else
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
|
||||
#endif
|
||||
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
|
||||
}
|
||||
|
|
@ -3791,11 +3852,7 @@ bool navigationTerrainFollowingEnabled(void)
|
|||
uint32_t distanceToFirstWP(void)
|
||||
{
|
||||
fpVector3_t startingWaypointPos;
|
||||
#ifdef USE_MULTI_MISSION
|
||||
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[posControl.loadedMultiMissionStartWP], GEO_ALT_RELATIVE);
|
||||
#else
|
||||
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
|
||||
#endif
|
||||
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[posControl.startWpIndex], GEO_ALT_RELATIVE);
|
||||
return calculateDistanceToDestination(&startingWaypointPos);
|
||||
}
|
||||
|
||||
|
|
@ -3832,7 +3889,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
|
|||
|
||||
// Don't allow arming if first waypoint is farther than configured safe distance
|
||||
if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
|
||||
if (distanceToFirstWP() > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) {
|
||||
if (distanceToFirstWP() > METERS_TO_CENTIMETERS(navConfig()->general.waypoint_safe_distance) && !checkStickPosition(YAW_HI)) {
|
||||
return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR;
|
||||
}
|
||||
}
|
||||
|
|
@ -3844,20 +3901,17 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
|
|||
* Can't jump beyond WP list
|
||||
* Only jump to geo-referenced WP types
|
||||
*/
|
||||
#ifdef USE_MULTI_MISSION
|
||||
// Only perform check when mission loaded at start of posControl.waypointList
|
||||
if (posControl.waypointCount && !posControl.loadedMultiMissionStartWP) {
|
||||
#else
|
||||
if (posControl.waypointCount) {
|
||||
#endif
|
||||
for (uint8_t wp = 0; wp < posControl.waypointCount; wp++){
|
||||
for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; 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)) {
|
||||
if (wp == posControl.startWpIndex || posControl.waypointList[wp].p1 >= posControl.waypointCount ||
|
||||
(posControl.waypointList[wp].p1 > (wp - posControl.startWpIndex - 2) && posControl.waypointList[wp].p1 < (wp - posControl.startWpIndex + 2)) || 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)) {
|
||||
uint16_t target = posControl.waypointList[wp].p1 + posControl.startWpIndex;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
@ -3941,7 +3995,7 @@ void missionPlannerSetWaypoint(void)
|
|||
posControl.waypointList[posControl.wpPlannerActiveWPIndex].lon = wpLLH.lon;
|
||||
posControl.waypointList[posControl.wpPlannerActiveWPIndex].alt = wpLLH.alt;
|
||||
posControl.waypointList[posControl.wpPlannerActiveWPIndex].p1 = posControl.waypointList[posControl.wpPlannerActiveWPIndex].p2 = 0;
|
||||
posControl.waypointList[posControl.wpPlannerActiveWPIndex].p3 = 1; // use absolute altitude datum
|
||||
posControl.waypointList[posControl.wpPlannerActiveWPIndex].p3 |= NAV_WP_ALTMODE; // use absolute altitude datum
|
||||
posControl.waypointList[posControl.wpPlannerActiveWPIndex].flag = NAV_WP_FLAG_LAST;
|
||||
posControl.waypointListValid = true;
|
||||
|
||||
|
|
@ -4112,9 +4166,9 @@ void navigationInit(void)
|
|||
posControl.waypointListValid = false;
|
||||
posControl.wpPlannerActiveWPIndex = 0;
|
||||
posControl.flags.wpMissionPlannerActive = false;
|
||||
posControl.startWpIndex = 0;
|
||||
#ifdef USE_MULTI_MISSION
|
||||
posControl.multiMissionCount = 0;
|
||||
posControl.loadedMultiMissionStartWP = 0;
|
||||
#endif
|
||||
/* Set initial surface invalid */
|
||||
posControl.actualState.surfaceMin = -1.0f;
|
||||
|
|
@ -4274,7 +4328,7 @@ bool navigationRTHAllowsLanding(void)
|
|||
{
|
||||
// WP mission RTH landing setting
|
||||
if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
|
||||
return posControl.waypointList[posControl.waypointCount - 1].p1 > 0;
|
||||
return posControl.waypointList[posControl.startWpIndex + posControl.waypointCount - 1].p1 > 0;
|
||||
}
|
||||
|
||||
// normal RTH landing setting
|
||||
|
|
@ -4291,8 +4345,7 @@ bool isNavLaunchEnabled(void)
|
|||
bool abortLaunchAllowed(void)
|
||||
{
|
||||
// allow NAV_LAUNCH_MODE to be aborted if throttle is low or throttle stick position is < launch idle throttle setting
|
||||
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
|
||||
return throttleStatus == THROTTLE_LOW || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
|
||||
return throttleStickIsLow() || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
|
||||
}
|
||||
|
||||
int32_t navigationGetHomeHeading(void)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue