diff --git a/.gitignore b/.gitignore index b2c3b9ff54..6b7c8734dc 100644 --- a/.gitignore +++ b/.gitignore @@ -22,6 +22,7 @@ cov-int* /downloads/ /debug/ /release/ +/*_SITL/ # script-generated files docs/Manual.pdf diff --git a/docs/Settings.md b/docs/Settings.md index adc17297c1..8641d0439a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2922,9 +2922,9 @@ Dive angle that airplane will use during final landing phase. During dive phase, --- -### nav_fw_launch_abort_deadband +### nav_fw_launch_land_abort_deadband -Launch abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch. +Launch and landing abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch or landing. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index eb5a07ddfb..fb0719efe1 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -123,8 +123,8 @@ #define PG_EZ_TUNE 1033 #define PG_LEDPIN_CONFIG 1034 #define PG_OSD_JOYSTICK_CONFIG 1035 -#define PG_INAV_END PG_OSD_JOYSTICK_CONFIG - +#define PG_FW_AUTOLAND_CONFIG 1036 +#define PG_INAV_END PG_FW_AUTOLAND_CONFIG // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 6e904b593f..a98b26a68f 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1312,18 +1312,23 @@ static void cliTempSensor(char *cmdline) #if defined(USE_SAFE_HOME) static void printSafeHomes(uint8_t dumpMask, const navSafeHome_t *navSafeHome, const navSafeHome_t *defaultSafeHome) { - const char *format = "safehome %u %u %d %d"; // uint8_t enabled, int32_t lat; int32_t lon + const char *format = "safehome %u %u %d %d %d %d %u %d %d"; // uint8_t enabled, int32_t lat; int32_t lon for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) { bool equalsDefault = false; if (defaultSafeHome) { equalsDefault = navSafeHome[i].enabled == defaultSafeHome[i].enabled && navSafeHome[i].lat == defaultSafeHome[i].lat - && navSafeHome[i].lon == defaultSafeHome[i].lon; + && navSafeHome[i].lon == defaultSafeHome[i].lon + && navSafeHome[i].approachDirection == defaultSafeHome[i].approachDirection + && navSafeHome[i].approachAltMSL == defaultSafeHome[i].approachAltMSL + && navSafeHome[i].landAltMSL == defaultSafeHome[i].landAltMSL + && navSafeHome[i].landApproachHeading1 == defaultSafeHome[i].landApproachHeading1 + && navSafeHome[i].landApproachHeading2 == defaultSafeHome[i].landApproachHeading2; cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, - defaultSafeHome[i].enabled, defaultSafeHome[i].lat, defaultSafeHome[i].lon); + defaultSafeHome[i].enabled, defaultSafeHome[i].lat, defaultSafeHome[i].lon, defaultSafeHome[i].approachAltMSL, defaultSafeHome[i].landAltMSL, defaultSafeHome[i].approachDirection, defaultSafeHome[i].landApproachHeading1, defaultSafeHome[i].landApproachHeading2); } cliDumpPrintLinef(dumpMask, equalsDefault, format, i, - navSafeHome[i].enabled, navSafeHome[i].lat, navSafeHome[i].lon); + navSafeHome[i].enabled, navSafeHome[i].lat, navSafeHome[i].lon, navSafeHome[i].approachAltMSL, navSafeHome[i].landAltMSL, navSafeHome[i].approachDirection, navSafeHome[i].landApproachHeading1, navSafeHome[i].landApproachHeading2); } } @@ -1334,7 +1339,7 @@ static void cliSafeHomes(char *cmdline) } else if (sl_strcasecmp(cmdline, "reset") == 0) { resetSafeHomes(); } else { - int32_t lat=0, lon=0; + int32_t lat=0, lon=0, approachAlt = 0, heading1 = 0, heading2 = 0, landDirection = 0, landAlt = 0; bool enabled=false; uint8_t validArgumentCount = 0; const char *ptr = cmdline; @@ -1354,16 +1359,54 @@ static void cliSafeHomes(char *cmdline) lon = fastA2I(ptr); validArgumentCount++; } + + if ((ptr = nextArg(ptr))) { + approachAlt = fastA2I(ptr); + validArgumentCount++; + } + + if ((ptr = nextArg(ptr))) { + landAlt = fastA2I(ptr); + validArgumentCount++; + } + + if ((ptr = nextArg(ptr))) { + landDirection = fastA2I(ptr); + + if (landDirection != 0 && landDirection != 1) { + cliShowParseError(); + return; + } + + validArgumentCount++; + } + + if ((ptr = nextArg(ptr))) { + heading1 = fastA2I(ptr); + validArgumentCount++; + } + + if ((ptr = nextArg(ptr))) { + heading2 = fastA2I(ptr); + validArgumentCount++; + } + if ((ptr = nextArg(ptr))) { // check for too many arguments validArgumentCount++; } - if (validArgumentCount != 3) { + + if (validArgumentCount != 8) { cliShowParseError(); } else { safeHomeConfigMutable(i)->enabled = enabled; safeHomeConfigMutable(i)->lat = lat; safeHomeConfigMutable(i)->lon = lon; + safeHomeConfigMutable(i)->approachAltMSL = approachAlt; + safeHomeConfigMutable(i)->landAltMSL = landAlt; + safeHomeConfigMutable(i)->approachDirection = (fwAutolandApproachDirection_e)landDirection; + safeHomeConfigMutable(i)->landApproachHeading1 = (int16_t)heading1; + safeHomeConfigMutable(i)->landApproachHeading2 = (int16_t)heading2; } } } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0982339493..18d8fafc61 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2836,15 +2836,6 @@ groups: field: fw.control_smoothness min: 0 max: 9 - - - name: nav_fw_land_dive_angle - description: "Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees" - default_value: 2 - field: fw.land_dive_angle - condition: NAV_FIXED_WING_LANDING - min: -20 - max: 20 - - name: nav_fw_launch_velocity description: "Forward velocity threshold for swing-launch detection [cm/s]" default_value: 300 @@ -2921,10 +2912,10 @@ groups: default_value: OFF field: fw.launch_manual_throttle type: bool - - name: nav_fw_launch_abort_deadband - description: "Launch abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch." + - name: nav_fw_launch_land_abort_deadband + description: "Launch and landing abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch or landing." default_value: 100 - field: fw.launch_abort_deadband + field: fw.launch_land_abort_deadband min: 2 max: 250 - name: nav_fw_allow_manual_thr_increase @@ -4092,3 +4083,51 @@ groups: field: osd_joystick_enter min: 0 max: 100 + + - name: PG_FW_AUTOLAND_CONFIG + type: navFwAutolandConfig_t + headers: ["navigation/navigation.h"] + condition: USE_SAFE_HOME + members: + - name: nav_fw_land_approach_length + description: "Angle of the final approach" + default_value: 50000 + field: approachLength + min: 100 + max: 100000 + - name: nav_fw_land_final_approach_pitch2throttle_mod + description: "Modifier for pitch to throttle at final approach. In Percent." + default_value: 100 + field: finalApproachPitchToThrottleMod + min: 100 + max: 400 + - name: nav_fw_land_glide_alt + description: "Initial altitude of the glide phase" + default_value: 500 + field: glideAltitude + min: 100 + max: 5000 + - name: nav_fw_land_flare_alt + description: "Initial altitude of the flare" + default_value: 2000 + field: flareAltitude + min: 0 + max: 10000 + - name: nav_fw_land_glide_pitch + description: "Pitch value for glide phase. In degrees." + default_value: 0 + field: glidePitch + min: 0 + max: 45 + - name: nav_fw_land_flare_pitch + description: "Pitch value for flare phase. In degrees" + default_value: 2 + field: flarePitch + min: 0 + max: 45 + - name: nav_fw_land_max_tailwind + description: "Max. tailwind (in cm/s) if no landing direction with downwind is available" + default_value: 140 + field: maxTailwind + min: 0 + max: 3000 diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index e0b13a5310..6420f6547b 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -38,6 +38,8 @@ #include "flight/imu.h" +#include "navigation/navigation_pos_estimator_private.h" + #include "io/gps.h" @@ -105,9 +107,9 @@ void updateWindEstimator(timeUs_t currentTimeUs) // Get current 3D velocity from GPS in cm/s // relative to earth frame - groundVelocity[X] = gpsSol.velNED[X]; - groundVelocity[Y] = gpsSol.velNED[Y]; - groundVelocity[Z] = gpsSol.velNED[Z]; + groundVelocity[X] = posEstimator.gps.vel.x; + groundVelocity[Y] = posEstimator.gps.vel.y; + groundVelocity[Z] = posEstimator.gps.vel.z; // Fuselage direction in earth frame fuselageDirection[X] = HeadVecEFFiltered.x; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 70e8f52445..4ea05d9cb2 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1007,10 +1007,10 @@ static const char * osdFailsafeInfoMessage(void) #if defined(USE_SAFE_HOME) static const char * divertingToSafehomeMessage(void) { - if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) { - return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); - } - return NULL; + if ((NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) || isFwLandInProgess()) { + return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); + } + return NULL; } #endif @@ -2269,8 +2269,9 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_FLYMODE: { char *p = "ACRO"; - - if (FLIGHT_MODE(FAILSAFE_MODE)) + if (isFwLandInProgess()) + p = "LAND"; + else if (FLIGHT_MODE(FAILSAFE_MODE)) p = "!FS!"; else if (FLIGHT_MODE(MANUAL_MODE)) p = "MANU"; @@ -5133,7 +5134,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter const char *invertedInfoMessage = NULL; if (ARMING_FLAG(ARMED)) { - if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { + if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || isFwLandInProgess()) { if (isWaypointMissionRTHActive()) { // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL); @@ -5165,9 +5166,13 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = messageBuf; } else { - const char *navStateMessage = navigationStateMessage(); - if (navStateMessage) { - messages[messageCount++] = navStateMessage; + if (canFwLandCanceld()) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS); + } else if (!isFwLandInProgess()) { + const char *navStateMessage = navigationStateMessage(); + if (navStateMessage) { + messages[messageCount++] = navStateMessage; + } } } #if defined(USE_SAFE_HOME) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 5f444ad454..e7c0f8b10b 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -123,6 +123,7 @@ #define OSD_MSG_ANGLEHOLD_ROLL "(ANGLEHOLD ROLL)" #define OSD_MSG_ANGLEHOLD_PITCH "(ANGLEHOLD PITCH)" #define OSD_MSG_ANGLEHOLD_LEVEL "(ANGLEHOLD LEVEL)" +#define OSD_MSG_MOVE_STICKS "MOVE STICKS TO ABORT" #ifdef USE_DEV_TOOLS #define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 2a832a7a4b..770ea69b00 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -49,6 +49,7 @@ #include "flight/imu.h" #include "flight/mixer_profile.h" #include "flight/pid.h" +#include "flight/wind_estimator.h" #include "io/beeper.h" #include "io/gps.h" @@ -76,6 +77,8 @@ #define FW_RTH_CLIMB_OVERSHOOT_CM 100 #define FW_RTH_CLIMB_MARGIN_MIN_CM 100 #define FW_RTH_CLIMB_MARGIN_PERCENT 15 +#define FW_LAND_LOITER_MIN_TIME 30000000 // usec (30 sec) +#define FW_LAND_LOITER_ALT_TOLERANCE 150 /*----------------------------------------------------------- * Compatibility for home position @@ -88,6 +91,18 @@ radar_pois_t radar_pois[RADAR_MAX_POIS]; #if defined(USE_SAFE_HOME) PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0); + +PG_REGISTER_WITH_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig, PG_FW_AUTOLAND_CONFIG, 0); +PG_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig, + .approachLength = SETTING_NAV_FW_LAND_APPROACH_LENGTH_DEFAULT, + .finalApproachPitchToThrottleMod = SETTING_NAV_FW_LAND_FINAL_APPROACH_PITCH2THROTTLE_MOD_DEFAULT, + .flareAltitude = SETTING_NAV_FW_LAND_FLARE_ALT_DEFAULT, + .flarePitch = SETTING_NAV_FW_LAND_FLARE_PITCH_DEFAULT, + .maxTailwind = SETTING_NAV_FW_LAND_MAX_TAILWIND_DEFAULT, + .glidePitch = SETTING_NAV_FW_LAND_GLIDE_PITCH_DEFAULT, +); + + #endif // waypoint 254, 255 are special waypoints @@ -193,9 +208,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .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 - // Fixed wing launch .launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s .launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G) @@ -210,10 +222,10 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees .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 + .launch_land_abort_deadband = SETTING_NAV_FW_LAUNCH_LAND_ABORT_DEADBAND_DEFAULT, // 100 us .allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT, - .useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_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 .wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions @@ -251,6 +263,7 @@ static void resetAltitudeController(bool useTerrainFollowing); static void resetPositionController(void); static void setupAltitudeController(void); static void resetHeadingController(void); +static void resetFwAutoland(void); void resetGCSFlags(void); static void setupJumpCounters(void); @@ -260,6 +273,7 @@ static void clearJumpCounters(void); static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint); static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos); void calculateInitialHoldPosition(fpVector3_t * pos); +void calculateFarAwayPos(fpVector3_t * farAwayPos, const fpVector3_t *start, int32_t bearing, int32_t distance); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance); static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing); bool isWaypointAltitudeReached(void); @@ -278,6 +292,11 @@ static bool rthAltControlStickOverrideCheck(unsigned axis); static void updateRthTrackback(bool forceSaveTrackPoint); static fpVector3_t * rthGetTrackbackPos(void); +static bool allowFwAutoland(void); +static int32_t calcWindDiff(int32_t heading, int32_t windHeading); +static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle); +static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos); + /*************************************************************************************************/ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState); @@ -316,6 +335,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState); static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { /** Idle state ******************************************************/ @@ -665,6 +690,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, + [NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME, } }, @@ -999,6 +1026,121 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, + }, + + [NAV_STATE_FW_LANDING_CLIMB_TO_LOITER] = { + .persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER, + .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, + .timeoutMs = 10, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | 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_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_FW_LANDING_LOITER, + [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_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT, + } + }, + + [NAV_STATE_FW_LANDING_LOITER] = { + .persistentId = NAV_PERSISTENT_ID_FW_LANDING_LOITER, + .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_LOITER, + .timeoutMs = 10, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | 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_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_LOITER, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_FW_LANDING_APPROACH, + [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_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT, + } + }, + + [NAV_STATE_FW_LANDING_APPROACH] = { + .persistentId = NAV_PERSISTENT_ID_FW_LANDING_APPROACH, + .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH, + .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_LAND_IN_PROGRESS, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_APPROACH, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_FW_LANDING_GLIDE, + [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_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT, + } + }, + + [NAV_STATE_FW_LANDING_GLIDE] = { + .persistentId = NAV_PERSISTENT_ID_FW_LANDING_GLIDE, + .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE, + .timeoutMs = 10, + .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW, + .mapToFlightModes = NAV_COURSE_HOLD_MODE, + .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_GLIDE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_FW_LANDING_FLARE, + [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_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT, + } + }, + + [NAV_STATE_FW_LANDING_FLARE] = { + .persistentId = NAV_PERSISTENT_ID_FW_LANDING_FLARE, + .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_FLARE, + .timeoutMs = 10, + .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW, + .mapToFlightModes = NAV_COURSE_HOLD_MODE, + .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_FLARE, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + } + }, + [NAV_STATE_FW_LANDING_ABORT] = { + .persistentId = NAV_PERSISTENT_ID_FW_LANDING_ABORT, + .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_ABORT, + .timeoutMs = 10, + .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 | NAV_RC_POS | NAV_RC_YAW, + .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, + .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_ABORT, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + } + }, }; static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) @@ -1030,6 +1172,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState resetAltitudeController(false); resetHeadingController(); resetPositionController(); + resetFwAutoland(); return NAV_FSM_EVENT_NONE; } @@ -1227,7 +1370,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(naviga static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState) { - UNUSED(previousState); if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive) posControl.rthState.rthLinearDescentActive = false; @@ -1239,9 +1381,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - if (STATE(FIXED_WING_LEGACY) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) { - // Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH - return NAV_FSM_EVENT_SWITCH_TO_IDLE; + if (previousState != NAV_STATE_FW_LANDING_ABORT) { + posControl.fwLandAborted = false; + if (STATE(FIXED_WING_LEGACY) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) { + // Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } } // If we have valid position sensor or configured to ignore it's loss at initial stage - continue @@ -1548,6 +1693,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; } + if (STATE(FIXED_WING_LEGACY)) { + if (allowFwAutoland()) { + resetFwAutoland(); + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING; + } else { + return NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; + } + } + float descentVelLimited = 0; fpVector3_t tmpHomePos = posControl.rthState.homeTmpWaypoint; @@ -1559,6 +1713,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF } // A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed else if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { + if ((posControl.flags.estAglStatus ) && posControl.actualState.agl.pos.z < 50.0f) { // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown // Do not allow descent velocity slower than -30cm/s so the landing detector works. descentVelLimited = navConfig()->general.land_minalt_vspd; @@ -1932,7 +2087,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF } // abort NAV_LAUNCH_MODE by moving sticks with low throttle or throttle stick < launch idle throttle - if (abortLaunchAllowed() && isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) { + if (abortLaunchAllowed() && isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) { abortFixedWingLaunch(); return NAV_FSM_EVENT_SWITCH_TO_IDLE; } @@ -2015,6 +2170,231 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigatio return NAV_FSM_EVENT_SUCCESS; } +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) { + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; + } + + if (ABS(getEstimatedActualPosition(Z) - posControl.fwLandAproachAltAgl) < (navConfig()->general.waypoint_enforce_altitude > 0 ? navConfig()->general.waypoint_enforce_altitude : FW_LAND_LOITER_ALT_TOLERANCE)) { + updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); + return NAV_FSM_EVENT_SUCCESS; + } + + fpVector3_t tmpHomePos = posControl.rthState.homePosition.pos; + tmpHomePos.z = posControl.fwLandAproachAltAgl; + + float timeToReachHomeAltitude = fabsf(tmpHomePos.z - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate; + + if (timeToReachHomeAltitude < 1) { + // we almost reached the target home altitude so set the desired altitude to the desired home altitude + setDesiredPosition(&tmpHomePos, 0, NAV_POS_UPDATE_Z); + } else { + float altitudeChangeDirection = tmpHomePos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; + updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); + } + + return NAV_FSM_EVENT_NONE; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navigationFSMState_t previousState) +{ + UNUSED(previousState); + /* If position sensors unavailable - land immediately (wait for timeout on GPS) */ + if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } + + if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) { + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; + } + + if (posControl.fwLandLoiterStartTime == 0) { + posControl.fwLandLoiterStartTime = micros(); + } else if (micros() - posControl.fwLandLoiterStartTime > FW_LAND_LOITER_MIN_TIME) { + if (isEstimatedWindSpeedValid()) { + + uint16_t windAngle = 0; + int32_t approachHeading = -1; + float windSpeed = getEstimatedHorizontalWindSpeed(&windAngle); + windAngle = wrap_36000(windAngle + 18000); + + // Ignore low wind speed, could be the error of the wind estimator + if (windSpeed < navFwAutolandConfig()->maxTailwind) { + if (safeHomeConfig(safehome_index)->landApproachHeading1 != 0) { + approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1)); + } else if ((safeHomeConfig(safehome_index)->landApproachHeading2 != 0) ) { + approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2)); + } else { + approachHeading = posControl.fwLandingDirection = -1; + } + } else { + int32_t heading1 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1), windAngle); + int32_t heading2 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2), windAngle); + + if (heading1 == heading2 || heading1 == wrap_36000(heading2 + 18000)) { + heading2 = -1; + } + + if (heading1 == -1 && heading2 >= 0) { + posControl.fwLandingDirection = heading2; + approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2); + } else if (heading1 >= 0 && heading2 == -1) { + posControl.fwLandingDirection = heading1; + approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1); + } else { + if (calcWindDiff(heading1, windAngle) < calcWindDiff(heading2, windAngle)) { + posControl.fwLandingDirection = heading1; + approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1); + } else { + posControl.fwLandingDirection = heading2; + approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2); + } + } + } + + if (posControl.fwLandingDirection >= 0) { + fpVector3_t tmpPos; + + int32_t finalApproachAlt = posControl.fwLandAproachAltAgl / 3 * 2; + int32_t dir = 0; + if (safeHomeConfig(safehome_index)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) { + dir = wrap_36000(approachHeading - 9000); + } else { + dir = wrap_36000(approachHeading + 9000); + } + + tmpPos = posControl.rthState.homePosition.pos; + tmpPos.z = posControl.fwLandAltAgl; + posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND] = tmpPos; + + calculateFarAwayPos(&tmpPos, &posControl.rthState.homePosition.pos, wrap_36000(posControl.fwLandingDirection + 18000), navFwAutolandConfig()->approachLength); + tmpPos.z = finalApproachAlt; + posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos; + + calculateFarAwayPos(&tmpPos, &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH], dir, navFwAutolandConfig()->approachLength / 2); + tmpPos.z = finalApproachAlt; + posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN] = tmpPos; + + setLandWaypoint(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN], &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH]); + posControl.fwLandCurrentWp = FW_AUTOLAND_WP_TURN; + posControl.fwLandState = FW_AUTOLAND_STATE_CROSSWIND; + + return NAV_FSM_EVENT_SUCCESS; + } else { + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + } else { + posControl.fwLandLoiterStartTime = micros(); + } + } + + fpVector3_t tmpPoint = posControl.rthState.homePosition.pos; + tmpPoint.z = posControl.fwLandAproachAltAgl; + setDesiredPosition(&tmpPoint, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + + return NAV_FSM_EVENT_NONE; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + /* If position sensors unavailable - land immediately (wait for timeout on GPS) */ + if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } + + if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) { + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; + } + + if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAltMSL + navFwAutolandConfig()->glideAltitude - GPS_home.alt) { + resetPositionController(); + posControl.fwLandState = FW_AUTOLAND_STATE_GLIDE; + return NAV_FSM_EVENT_SUCCESS; + } else if (isWaypointReached(&posControl.fwLandWaypoint[posControl.fwLandCurrentWp], &posControl.activeWaypoint.bearing)) { + if (posControl.fwLandCurrentWp == FW_AUTOLAND_WP_TURN) { + setLandWaypoint(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH], &posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND]); + posControl.fwLandCurrentWp = FW_AUTOLAND_WP_FINAL_APPROACH; + posControl.fwLandState = FW_AUTOLAND_STATE_BASE_LEG; + return NAV_FSM_EVENT_NONE; + } else if (posControl.fwLandCurrentWp == FW_AUTOLAND_WP_FINAL_APPROACH) { + setLandWaypoint(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND], NULL); + posControl.fwLandCurrentWp = FW_AUTOLAND_WP_LAND; + posControl.fwLandState = FW_AUTOLAND_STATE_FINAL_APPROACH; + return NAV_FSM_EVENT_NONE; + } + } + + fpVector3_t tmpWaypoint; + tmpWaypoint.x = posControl.activeWaypoint.pos.x; + tmpWaypoint.y = posControl.activeWaypoint.pos.y; + tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance), + posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f, + posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); + setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + + return NAV_FSM_EVENT_NONE; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) { + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; + } + + // Surface sensor present? + if (posControl.flags.estAglStatus == EST_TRUSTED) { + if (getEstimatedActualPosition(Z) <= posControl.fwLandAltAgl + navFwAutolandConfig()->flareAltitude) { + posControl.cruise.course = posControl.fwLandingDirection; + posControl.cruise.previousCourse = posControl.cruise.course; + posControl.cruise.lastCourseAdjustmentTime = 0; + posControl.fwLandState = FW_AUTOLAND_STATE_FLARE; + return NAV_FSM_EVENT_SUCCESS; + } + } else if (isLandingDetected()) { + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + + if (!posControl.fwLandWpReached) { + if (calculateDistanceToDestination(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND]) > navConfig()->general.waypoint_radius / 2) { + posControl.cruise.course = calculateBearingToDestination(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND]); + posControl.cruise.previousCourse = posControl.cruise.course; + posControl.cruise.lastCourseAdjustmentTime = 0; + } else { + posControl.fwLandWpReached = true; + } + } + + setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING); + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); + return NAV_FSM_EVENT_NONE; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + if (isLandingDetected()) { + return NAV_FSM_EVENT_SUCCESS; + } + setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING); + + return NAV_FSM_EVENT_NONE; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState) +{ + UNUSED(previousState); + posControl.fwLandAborted = true; + return NAV_FSM_EVENT_SWITCH_TO_RTH; +} + static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState) { navigationFSMState_t previousState; @@ -2971,11 +3351,16 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag } } +void calculateFarAwayPos(fpVector3_t *farAwayPos, const fpVector3_t *start, int32_t bearing, int32_t distance) +{ + farAwayPos->x = start->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(bearing)); + farAwayPos->y = start->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(bearing)); + farAwayPos->z = start->z; +} + void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance) { - farAwayPos->x = navGetCurrentActualPositionAndVelocity()->pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(bearing)); - farAwayPos->y = navGetCurrentActualPositionAndVelocity()->pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(bearing)); - farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z; + calculateFarAwayPos(farAwayPos, &navGetCurrentActualPositionAndVelocity()->pos, bearing, distance); } /*----------------------------------------------------------- @@ -3731,7 +4116,7 @@ void applyWaypointNavigationAndAltitudeHold(void) posControl.activeRthTBPointIndex = -1; posControl.flags.rthTrackbackActive = false; posControl.rthTBWrapAroundCounter = -1; - + posControl.fwLandAborted = false; return; } @@ -3739,6 +4124,10 @@ void applyWaypointNavigationAndAltitudeHold(void) posControl.flags.horizontalPositionDataConsumed = false; posControl.flags.verticalPositionDataConsumed = false; + if (!isFwLandInProgess()) { + posControl.fwLandState = FW_AUTOLAND_LC_STATE_IDLE; + } + /* Process controllers */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); if (STATE(ROVER) || STATE(BOAT)) { @@ -4595,25 +4984,72 @@ int32_t navigationGetHeadingError(void) return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog); } -int8_t navCheckActiveAngleHoldAxis(void) +static void resetFwAutoland(void) { - int8_t activeAxis = -1; + posControl.fwLandAltAgl = safeHomeConfig(safehome_index)->landAltMSL - GPS_home.alt; + posControl.fwLandAproachAltAgl = safeHomeConfig(safehome_index)->approachAltMSL + posControl.fwLandAltAgl - GPS_home.alt; + posControl.fwLandLoiterStartTime = 0; + posControl.fwLandWpReached = false; +} - if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) { - navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); - bool altholdActive = stateFlags & NAV_REQUIRE_ANGLE_FW && !(stateFlags & NAV_REQUIRE_ANGLE); +static bool allowFwAutoland(void) +{ + return !posControl.fwLandAborted && safehome_index >= 0 && (safeHomeConfig(safehome_index)->landApproachHeading1 != 0 || safeHomeConfig(safehome_index)->landApproachHeading2 != 0); +} - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !FLIGHT_MODE(NAV_ALTHOLD_MODE)) { - activeAxis = FD_PITCH; - } else if (altholdActive) { - activeAxis = FD_ROLL; - } +static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle) +{ + if (approachHeading == 0) { + return -1; } - - return activeAxis; + + int32_t windRelToRunway = wrap_36000(windAngle - ABS(approachHeading)); + // Headwind? + if (windRelToRunway >= 27000 || windRelToRunway <= 9000) { + return wrap_36000(ABS(approachHeading)); + } + + if (approachHeading > 0) { + return wrap_36000(approachHeading + 18000); + } + + return -1; } -uint8_t getActiveWpNumber(void) +static int32_t calcWindDiff(int32_t heading, int32_t windHeading) { - return NAV_Status.activeWpNumber; + return ABS(wrap_18000(windHeading - heading)); } + +static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos) +{ + calculateAndSetActiveWaypointToLocalPosition(pos); + + if (navConfig()->fw.wp_turn_smoothing && nextWpPos != NULL) { + int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, nextWpPos); + posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing); + } else { + posControl.activeWaypoint.nextTurnAngle = -1; + } + + posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); + posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; + posControl.wpAltitudeReached = false; +} + +bool isFwLandInProgess(void) +{ + return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER + || posControl.navState == NAV_STATE_FW_LANDING_LOITER + || posControl.navState == NAV_STATE_FW_LANDING_APPROACH + || posControl.navState == NAV_STATE_FW_LANDING_GLIDE + || posControl.navState == NAV_STATE_FW_LANDING_FLARE; +} + +bool canFwLandCanceld(void) +{ + return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER + || posControl.navState == NAV_STATE_FW_LANDING_LOITER + || posControl.navState == NAV_STATE_FW_LANDING_APPROACH + || posControl.navState == NAV_STATE_FW_LANDING_GLIDE; +} \ No newline at end of file diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 0541c6cd8a..da5dd0cf0a 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -44,10 +44,28 @@ void onNewGPSData(void); #define MAX_SAFE_HOMES 8 +typedef enum { + FW_AUTOLAND_APPROACH_DIRECTION_LEFT, + FW_AUTOLAND_APPROACH_DIRECTION_RIGHT +} fwAutolandApproachDirection_e; + +typedef enum { + FW_AUTOLAND_LC_STATE_IDLE, + FW_AUTOLAND_STATE_CROSSWIND, + FW_AUTOLAND_STATE_BASE_LEG, + FW_AUTOLAND_STATE_FINAL_APPROACH, + FW_AUTOLAND_STATE_GLIDE, + FW_AUTOLAND_STATE_FLARE +} fwAutolandState_t; typedef struct { uint8_t enabled; int32_t lat; int32_t lon; + int32_t approachAltMSL; + int32_t landAltMSL; + fwAutolandApproachDirection_e approachDirection; + int16_t landApproachHeading1; + int16_t landApproachHeading2; } navSafeHome_t; typedef enum { @@ -58,8 +76,26 @@ typedef enum { PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig); -void resetSafeHomes(void); // remove all safehomes -bool findNearestSafeHome(void); // Find nearest safehome +typedef struct navFwAutolandConfig_s +{ + uint32_t approachLength; + uint16_t finalApproachPitchToThrottleMod; + uint16_t glideAltitude; + uint16_t flareAltitude; + uint8_t flarePitch; + uint16_t maxTailwind; + uint8_t glidePitch; +} navFwAutolandConfig_t; + +PG_DECLARE(navFwAutolandConfig_t, navFwAutolandConfig); + +extern int8_t safehome_index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise +extern uint32_t safehome_distance; // distance to the nearest safehome +extern bool safehome_applied; // whether the safehome has been applied to home. + +void resetSafeHomes(void); // remove all safehomes +bool findNearestSafeHome(void); // Find nearest safehome + #endif // defined(USE_SAFE_HOME) #ifndef NAV_MAX_WAYPOINTS @@ -302,7 +338,6 @@ typedef struct navConfig_s { uint16_t minThrottleDownPitchAngle; // Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle. [decidegrees] uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) - int8_t land_dive_angle; uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection uint16_t launch_accel_thresh; // Acceleration threshold for launch detection (cm/s/s) uint16_t launch_time_thresh; // Time threshold for launch detection (ms) @@ -316,7 +351,8 @@ typedef struct navConfig_s { uint8_t launch_climb_angle; // Target climb angle for launch (deg) uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] bool launch_manual_throttle; // Allows launch with manual throttle control - uint8_t launch_abort_deadband; // roll/pitch stick movement deadband for launch abort + uint8_t launch_land_abort_deadband; // roll/pitch stick movement deadband for launch abort + uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled bool allow_manual_thr_increase; bool useFwNavYawControl; uint8_t yawControlDeadband; @@ -637,6 +673,9 @@ uint8_t getActiveWpNumber(void); */ int32_t navigationGetHomeHeading(void); +bool isFwLandInProgess(void); +bool canFwLandCanceld(void); + /* Compatibility data */ extern navSystemStatus_t NAV_Status; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index f1bff8f804..1894d7374e 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -588,13 +588,18 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs // Apply low-pass filter to pitch angle to smooth throttle correction int16_t filteredPitch = (int16_t)pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr)); + int16_t pitchToThrottle = currentBatteryProfile->nav.fw.pitch_to_throttle; + if (pitch < 0 && posControl.fwLandState == FW_AUTOLAND_STATE_FINAL_APPROACH) { + pitchToThrottle *= navFwAutolandConfig()->finalApproachPitchToThrottleMod / 100.0f; + } + if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) { // Unfiltered throttle correction outside of pitch deadband - return DECIDEGREES_TO_DEGREES(pitch) * currentBatteryProfile->nav.fw.pitch_to_throttle; + return DECIDEGREES_TO_DEGREES(pitch) * pitchToThrottle; } else { // Filtered throttle correction inside of pitch deadband - return DECIDEGREES_TO_DEGREES(filteredPitch) * currentBatteryProfile->nav.fw.pitch_to_throttle; + return DECIDEGREES_TO_DEGREES(filteredPitch) * pitchToThrottle; } } @@ -620,7 +625,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs); #ifdef NAV_FIXED_WING_LANDING - if (navStateFlags & NAV_CTL_LAND) { + if ((navStateFlags & NAV_CTL_LAND) || isFwLandInProgess()) { // During LAND we do not allow to raise THROTTLE when nose is up to reduce speed throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0); } else { @@ -654,24 +659,18 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat } #ifdef NAV_FIXED_WING_LANDING - /* - * Then altitude is below landing slowdown min. altitude, enable final approach procedure - * TODO refactor conditions in this metod if logic is proven to be correct - */ - if (navStateFlags & NAV_CTL_LAND || STATE(LANDING_DETECTED)) { - int32_t finalAltitude = navConfig()->general.land_slowdown_minalt + posControl.rthState.homeTmpWaypoint.z; - if ((posControl.flags.estAltStatus >= EST_USABLE && navGetCurrentActualPositionAndVelocity()->pos.z <= finalAltitude) || - (posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) { + if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE || STATE(LANDING_DETECTED)) { + // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled + rcCommand[THROTTLE] = getThrottleIdleValue(); + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); - // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled - ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); + if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE) { + rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->glidePitch), pidProfile()->max_angle_inclination[FD_PITCH]); + } - // Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position - rcCommand[ROLL] = 0; - - // Stabilize PITCH angle into shallow dive as specified by the nav_fw_land_dive_angle setting (default value is 2 - defined in navigation.c). - rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navConfig()->fw.land_dive_angle), pidProfile()->max_angle_inclination[FD_PITCH]); + if (posControl.navState == NAV_STATE_FW_LANDING_FLARE) { + rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->flarePitch), pidProfile()->max_angle_inclination[FD_PITCH]); } } #endif diff --git a/src/main/navigation/navigation_fw_autoland.c b/src/main/navigation/navigation_fw_autoland.c new file mode 100644 index 0000000000..4f97dbf30f --- /dev/null +++ b/src/main/navigation/navigation_fw_autoland.c @@ -0,0 +1,445 @@ +/* + * This file is part of INAV. + * + * INAV is free software. You can redistribute this software + * and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * INAV is distributed in the hope that they will be + * useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#if defined(USE_SAFE_HOME) && defined(USE_FW_AUTOLAND) + +#include + +#include "common/utils.h" +#include "flight/wind_estimator.h" +#include "fc/rc_controls.h" +#include "fc/settings.h" +#include "rx/rx.h" +#include "flight/mixer.h" +#include "navigation/navigation_private.h" +#include "config/parameter_group_ids.h" + +#define LOITER_MIN_TIME 30000000 // usec (30 sec) +#define TARGET_ALT_TOLERANCE 150 + +#define LAND_WP_TURN 0 +#define LAND_WP_FINAL_APPROACH 1 +#define LAND_WP_LAND 2 + +typedef enum { + FW_AUTOLAND_EVENT_NONE, + FW_AUTOLAND_EVENT_SUCCSESS, + FW_AUTOLAND_EVENT_ABORT, + FW_AUTOLAND_EVENT_COUNT +} fwAutolandEvent_t; + +typedef struct fixedWingAutolandStateDescriptor_s { + fwAutolandEvent_t (*onEntry)(timeUs_t currentTimeUs); + fwAutolandState_t onEvent[FW_AUTOLAND_EVENT_COUNT]; + fwAutolandMessageState_t message; +} fixedWingAutolandStateDescriptor_t; + +typedef struct fixedWingAutolandData_s { + timeUs_t currentStateTimeUs; + fwAutolandState_t currentState; + timeUs_t loiterStartTime; + fpVector3_t waypoint[3]; + fpVector3_t flareWp; + uint32_t finalApproachLength; +} fixedWingAutolandData_t; + +static EXTENDED_FASTRAM fixedWingAutolandData_t fwAutoland; + +static fwAutolandEvent_t fwAutolandState_ABOVE_LOITER_ALT(timeUs_t currentTimeUs); +static fwAutolandEvent_t fwAutolandState_LOITER(timeUs_t currentTimeUs); +static fwAutolandEvent_t fwAutolandState_DOWNWIND(timeUs_t currentTimeUs); +static fwAutolandEvent_t fwAutolandState_BASE(timeUs_t currentTimeUs); +static fwAutolandEvent_t fwAutolandState_FINAL_APPROACH(timeUs_t currentTimeUs); +static fwAutolandEvent_t fwAutolandState_GLIDE(timeUs_t currentTimeUs); +static fwAutolandEvent_t fwAutolandState_FLARE(timeUs_t currentTimeUs); +static fwAutolandEvent_t fwAutolandState_ABORT(timeUs_t currentTimeUs); + +static const fixedWingAutolandStateDescriptor_t autolandStateMachine[FW_AUTOLAND_STATE_COUNT] = { + [FW_AUTOLAND_STATE_ABOVE_LOITER_ALT] = { + .onEntry = fwAutolandState_ABOVE_LOITER_ALT, + .onEvent = { + [FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_LOITER, + [FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT, + }, + .message = FW_AUTOLAND_MESSAGE_ABOVE_LOITER_ALT + }, + [FW_AUTOLAND_STATE_LOITER] = { + .onEntry = fwAutolandState_LOITER, + .onEvent = { + [FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_DOWNWIND, + [FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT, + }, + .message = FW_AUTOLAND_MESSAGE_LOITER + }, + [FW_AUTOLAND_STATE_DOWNWIND] = { + .onEntry = fwAutolandState_DOWNWIND, + .onEvent = { + [FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_BASE, + [FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT, + }, + .message = FW_AUTOLAND_MESSAGE_LOITER + }, + [FW_AUTOLAND_STATE_BASE] = { + .onEntry = fwAutolandState_BASE, + .onEvent = { + [FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_FINAL_APPROACH, + [FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT, + }, + .message = FW_AUTOLAND_MESSAGE_LOITER + }, + [FW_AUTOLAND_STATE_FINAL_APPROACH] = { + .onEntry = fwAutolandState_FINAL_APPROACH, + .onEvent = { + [FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_GLIDE, + [FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT, + }, + .message = FW_AUTOLAND_MESSAGE_LOITER + }, + [FW_AUTOLAND_STATE_GLIDE] = { + .onEntry = fwAutolandState_GLIDE, + .onEvent = { + [FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_FLARE, + [FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT, + }, + .message = FW_AUTOLAND_MESSAGE_LOITER + }, + [FW_AUTOLAND_STATE_FLARE] = { + .onEntry = fwAutolandState_FLARE, + .onEvent = { + [FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_LANDED, + [FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT, + }, + .message = FW_AUTOLAND_MESSAGE_LOITER + } + +}; + +PG_REGISTER_WITH_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig, PG_FW_AUTOLAND_CONFIG, 0); +PG_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig, + .approachAngle = SETTING_NAV_FW_LAND_APPROACH_ANGLE_DEFAULT, + .glideAltitude = SETTING_NAV_FW_LAND_GLIDE_ALT_DEFAULT, + .flareAltitude = SETTING_NAV_FW_LAND_FLARE_ALT_DEFAULT, + .flarePitch = SETTING_NAV_FW_LAND_FLARE_PITCH_DEFAULT, + .maxTailwind = SETTING_NAV_FW_LAND_MAX_TAILWIND_DEFAULT, +); + +static uint32_t getFinalApproachHeading(int32_t approachHeading, int32_t windAngle, int32_t *windAngleRelToRunway) +{ + if (approachHeading == 0) { + return 0; + } + + *windAngleRelToRunway = wrap_36000(windAngle - ABS(approachHeading)); + if (*windAngleRelToRunway > 27000 || *windAngleRelToRunway < 9000) { + return approachHeading; + } + + if (approachHeading > 0) { + return wrap_36000(approachHeading + 18000); + } + + return 0; +} + +static int32_t calcApproachLength(int32_t finalApproachAlt, int16_t glidePitch) +{ + return finalApproachAlt * 1.0f / sin_approx(DEGREES_TO_RADIANS(glidePitch)) * sin_approx(DEGREES_TO_RADIANS(90 - glidePitch)); +} + +static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos) +{ + if (posControl.activeWaypoint.pos.x == 0 && posControl.activeWaypoint.pos.y == 0) { + posControl.activeWaypoint.bearing = calculateBearingToDestination(pos); + } else { + posControl.activeWaypoint.bearing = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos); + } + + posControl.activeWaypoint.pos = *pos; + setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.bearing, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + + if (navConfig()->fw.wp_turn_smoothing && nextWpPos != NULL) { + int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(pos, nextWpPos); + posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing); + } else { + posControl.activeWaypoint.nextTurnAngle = -1; + } + + posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); + posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; + posControl.wpAltitudeReached = false; +} + +static void checkLandWpAndUpdateZ(void) +{ + fpVector3_t tmpWaypoint; + tmpWaypoint.x = posControl.activeWaypoint.pos.x; + tmpWaypoint.y = posControl.activeWaypoint.pos.y; + tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance), + posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f, + posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); + setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); +} + +static bool isLandWpReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing) +{ + posControl.wpDistance = calculateDistanceToDestination(waypointPos); + + /* + if (getFwAutolandState() == FW_AUTOLAND_STATE_BASE) { + return calculateDistanceToDestination(&fwAutoland.waypoint[LAND_WP_LAND]) <= fwAutoland.finalApproachLength - navConfig()->fw.loiter_radius; + } + */ + + if (posControl.flags.wpTurnSmoothingActive) { + posControl.flags.wpTurnSmoothingActive = false; + return true; + } + + uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000; + if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearing) { + return true; + } + + return posControl.wpDistance <= (navConfig()->general.waypoint_radius); +} + +static fwAutolandEvent_t fwAutolandState_ABOVE_LOITER_ALT(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + int32_t approachAltAbs = safeHomeConfig(safehome_index)->approachAltMSL - GPS_home.alt; + + if (ABS(getEstimatedActualPosition(Z) - approachAltAbs) < TARGET_ALT_TOLERANCE) { + return FW_AUTOLAND_EVENT_SUCCSESS; + } else { + fpVector3_t tmpPoint = posControl.rthState.homePosition.pos; + tmpPoint.z = approachAltAbs; + setDesiredPosition(&tmpPoint, 0, NAV_POS_UPDATE_Z); + } + + return FW_AUTOLAND_EVENT_NONE; +} + +static fwAutolandEvent_t fwAutolandState_LOITER(timeUs_t currentTimeUs) +{ +#ifndef USE_WIND_ESTIMATOR + return FW_AUTOLAND_EVENT_ABORT; +#endif + + int32_t landAltAbs = safeHomeConfig(safehome_index)->landAltMSL - GPS_home.alt; + int32_t approachAltAbs = safeHomeConfig(safehome_index)->approachAltMSL + landAltAbs - GPS_home.alt; + if (fwAutoland.loiterStartTime == 0) { + fwAutoland.loiterStartTime = currentTimeUs; + } else if (micros() - fwAutoland.loiterStartTime > LOITER_MIN_TIME) { + if (isEstimatedWindSpeedValid()) { + + uint16_t windAngle = 0; + uint32_t landingDirection = 0; + float windSpeed = getEstimatedHorizontalWindSpeed(&windAngle); + windAngle = wrap_36000(windAngle + 18000); + + int32_t windAngel1 = 0, windAngel2 = 0; + int32_t heading1 = getFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1), windAngle, &windAngel1); + int32_t heading2 = getFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2), windAngle, &windAngel2); + + if (heading1 == 0 && heading2 == 0 && windSpeed < navFwAutolandConfig()->maxTailwind) { + heading1 = safeHomeConfig(safehome_index)->landApproachHeading1; + heading2 = safeHomeConfig(safehome_index)->landApproachHeading2; + } + + if (heading1 == 0 && heading2 > 0) { + landingDirection = heading2; + } else if (heading1 > 0 && heading2 == 0) { + landingDirection = heading1; + } else { + if (windAngel1 < windAngel2) { + landingDirection = heading1; + } + else { + landingDirection = heading2; + } + } + + if (landingDirection != 0) { + fpVector3_t tmpPos; + + int32_t finalApproachAlt = approachAltAbs / 3 * 2; + fwAutoland.finalApproachLength = calcApproachLength(finalApproachAlt, navFwAutolandConfig()->approachAngle); + + int32_t dir = 0; + if (safeHomeConfig(safehome_index)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) { + dir = wrap_36000(landingDirection - 9000); + } else { + dir = wrap_36000(landingDirection + 9000); + } + + tmpPos = posControl.rthState.homePosition.pos; + tmpPos.z = landAltAbs; + fwAutoland.waypoint[LAND_WP_LAND] = tmpPos; + + fpVector3_t tmp2; + calculateFarAwayPos(&tmp2, &posControl.rthState.homePosition.pos, wrap_36000(landingDirection + 18000), fwAutoland.finalApproachLength); + calculateFarAwayPos(&tmpPos, &tmp2, dir, navConfig()->fw.loiter_radius); + tmp2.z = finalApproachAlt; + fwAutoland.waypoint[LAND_WP_FINAL_APPROACH] = tmp2; + + calculateFarAwayPos(&tmpPos, &fwAutoland.waypoint[LAND_WP_FINAL_APPROACH], dir, (fwAutoland.finalApproachLength / 2)); + tmpPos.z = finalApproachAlt; + fwAutoland.waypoint[LAND_WP_TURN] = tmpPos; + + setLandWaypoint(&fwAutoland.waypoint[LAND_WP_TURN], &fwAutoland.waypoint[LAND_WP_FINAL_APPROACH]); + + return FW_AUTOLAND_EVENT_SUCCSESS; + } else { + return FW_AUTOLAND_EVENT_ABORT; + } + } else { + fwAutoland.loiterStartTime = currentTimeUs; + } + } + + fpVector3_t tmpPoint = posControl.rthState.homePosition.pos; + tmpPoint.z = approachAltAbs; + setDesiredPosition(&tmpPoint, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + + return FW_AUTOLAND_EVENT_NONE; +} + + +static fwAutolandEvent_t fwAutolandState_DOWNWIND(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + if (isLandWpReached(&fwAutoland.waypoint[LAND_WP_TURN], &posControl.activeWaypoint.bearing)) { + setLandWaypoint(&fwAutoland.waypoint[LAND_WP_FINAL_APPROACH], &fwAutoland.waypoint[LAND_WP_LAND]); + return FW_AUTOLAND_EVENT_SUCCSESS; + } + + checkLandWpAndUpdateZ(); + + return FW_AUTOLAND_EVENT_NONE; +} + +static fwAutolandEvent_t fwAutolandState_BASE(timeUs_t currentTimeUs) +{ + if (isLandWpReached(&fwAutoland.waypoint[LAND_WP_FINAL_APPROACH], &posControl.activeWaypoint.bearing)) { + setLandWaypoint(&fwAutoland.waypoint[LAND_WP_LAND], NULL); + /* + resetGCSFlags(); + resetPositionController(); + resetAltitudeController(false); + setupAltitudeController(); + posControl.cruise.course = calculateBearingToDestination(&fwAutoland.waypoint[LAND_WP_LAND]); + */ + return FW_AUTOLAND_EVENT_SUCCSESS; + } + + checkLandWpAndUpdateZ(); + + return FW_AUTOLAND_EVENT_NONE; +} + +static fwAutolandEvent_t fwAutolandState_FINAL_APPROACH(timeUs_t currentTimeUs) +{ + if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAltMSL + navFwAutolandConfig()->glideAltitude - GPS_home.alt) { + setLandWaypoint(&fwAutoland.waypoint[LAND_WP_LAND], NULL); + return FW_AUTOLAND_EVENT_SUCCSESS; + } + posControl.wpDistance = calculateDistanceToDestination(&fwAutoland.waypoint[LAND_WP_LAND]); + checkLandWpAndUpdateZ(); + return FW_AUTOLAND_EVENT_NONE; +} + +static fwAutolandEvent_t fwAutolandState_GLIDE(timeUs_t currentTimeUs) +{ + if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAltMSL + navFwAutolandConfig()->flareAltitude - GPS_home.alt) { + return FW_AUTOLAND_EVENT_SUCCSESS; + } + posControl.wpDistance = calculateDistanceToDestination(&fwAutoland.waypoint[LAND_WP_LAND]); + checkLandWpAndUpdateZ(); + return FW_AUTOLAND_EVENT_NONE; +} + +static fwAutolandEvent_t fwAutolandState_FLARE(timeUs_t currentTimeUs) +{ + rcCommand[THROTTLE] = getThrottleIdleValue(); + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); + rxOverrideRcChannel(PITCH, -navFwAutolandConfig()->flarePitch); + + if (isLandingDetected()) { + rxOverrideRcChannel(PITCH, -1); + return FW_AUTOLAND_EVENT_SUCCSESS; + } + return FW_AUTOLAND_EVENT_NONE; +} + +static fwAutolandEvent_t fwAutolandState_ABORT(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + return FW_AUTOLAND_EVENT_NONE; +} + +static void setCurrentState(fwAutolandState_t nextState, timeUs_t currentTimeUs) +{ + fwAutoland.currentState = nextState; + fwAutoland.currentStateTimeUs = currentTimeUs; +} + +// Public methods +fwAutolandState_t getFwAutolandState(void) +{ + return fwAutoland.currentState; +} + +bool isFwAutolandActive(void) +{ + return fwAutoland.currentState >= FW_AUTOLAND_STATE_LOITER; +} + +bool allowFwAutoland(void) +{ + return safehome_index >= 0 && (safeHomeConfig(safehome_index)->landApproachHeading1 != 0 || safeHomeConfig(safehome_index)->landApproachHeading2 != 0); +} + +void resetFwAutolandController(timeUs_t currentTimeUs) +{ + setCurrentState(FW_AUTOLAND_STATE_ABOVE_LOITER_ALT, currentTimeUs); + rxOverrideRcChannel(PITCH, -1); + fwAutoland.loiterStartTime = 0; +} + +void applyFixedWingAutolandController(timeUs_t currentTimeUs) +{ + while (autolandStateMachine[fwAutoland.currentState].onEntry) { + fwAutolandEvent_t newEvent = autolandStateMachine[fwAutoland.currentState].onEntry(currentTimeUs); + if (newEvent == FW_AUTOLAND_EVENT_NONE) { + break; + } + setCurrentState(autolandStateMachine[fwAutoland.currentState].onEvent[newEvent], currentTimeUs); + } +} + +#endif \ No newline at end of file diff --git a/src/main/navigation/navigation_fw_autoland.h b/src/main/navigation/navigation_fw_autoland.h new file mode 100644 index 0000000000..733d4e3b1a --- /dev/null +++ b/src/main/navigation/navigation_fw_autoland.h @@ -0,0 +1,66 @@ +/* + * This file is part of INAV. + * + * INAV is free software. You can redistribute this software + * and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * INAV is distributed in the hope that they will be + * useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include "drivers/time.h" +#include "config/parameter_group.h" + +typedef enum { + FW_AUTOLAND_APPROACH_DIRECTION_LEFT, + FW_AUTOLAND_APPROACH_DIRECTION_RIGHT +} fwAutolandApproachDirection_e; + +typedef struct navFwAutolandConfig_s +{ + uint8_t approachAngle; + uint16_t glideAltitude; + uint16_t flareAltitude; + uint16_t flarePitch; + uint16_t maxTailwind; +} navFwAutolandConfig_t; + +PG_DECLARE(navFwAutolandConfig_t, navFwAutolandConfig); + +typedef enum { + FW_AUTOLAND_STATE_ABOVE_LOITER_ALT, + FW_AUTOLAND_STATE_LOITER, + FW_AUTOLAND_STATE_DOWNWIND, + FW_AUTOLAND_STATE_BASE, + FW_AUTOLAND_STATE_FINAL_APPROACH, + FW_AUTOLAND_STATE_GLIDE, + FW_AUTOLAND_STATE_FLARE, + FW_AUTOLAND_STATE_SWITCH_TO_EMERGENCY_LANDING, + FW_AUTOLAND_STATE_ABORT, + FW_AUTOLAND_STATE_LANDED, + FW_AUTOLAND_STATE_COUNT +} fwAutolandState_t; + +typedef enum { + FW_AUTOLAND_MESSAGE_ABOVE_LOITER_ALT, + FW_AUTOLAND_MESSAGE_LOITER, + FW_AUTOLAND_MESSAGE_ABORT, +} fwAutolandMessageState_t; + +fwAutolandState_t getFwAutolandState(void); +void resetFwAutolandController(timeUs_t currentTimeUs); +bool allowFwAutoland(void); +bool isFwAutolandActive(void); +void applyFixedWingAutolandController(timeUs_t currentTimeUs); \ No newline at end of file diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index fe0af4d55a..5708e5dcaa 100644 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -251,7 +251,7 @@ static inline bool isLaunchMaxAltitudeReached(void) static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) { return (initialTime + currentStateElapsedMs(currentTimeUs)) >= navConfig()->fw.launch_min_time && - isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband); + isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband); } static inline bool isProbablyNotFlying(void) @@ -420,7 +420,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t if (throttleStickIsLow()) { fwLaunch.currentStateTimeUs = currentTimeUs; fwLaunch.pitchAngle = 0; - if (isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) { + if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) { return FW_LAUNCH_EVENT_ABORT; } } else { @@ -454,7 +454,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; - if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) { + if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) { return FW_LAUNCH_EVENT_SUCCESS; // End launch go to FW_LAUNCH_STATE_FLYING state } else { diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 2bd72d7ae1..527d6d9a92 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -158,6 +158,7 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_CRUISE, NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ, NAV_FSM_EVENT_SWITCH_TO_MIXERAT, + NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING, NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_2, // State-specific event @@ -173,6 +174,7 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4, NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_5, NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_6, + NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT = NAV_FSM_EVENT_STATE_SPECIFIC_6, NAV_FSM_EVENT_COUNT, } navigationFSMEvent_t; @@ -233,6 +235,12 @@ typedef enum { NAV_PERSISTENT_ID_MIXERAT_INITIALIZE = 39, NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS = 40, NAV_PERSISTENT_ID_MIXERAT_ABORT = 41, + NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER = 42, + NAV_PERSISTENT_ID_FW_LANDING_LOITER = 43, + NAV_PERSISTENT_ID_FW_LANDING_APPROACH = 44, + NAV_PERSISTENT_ID_FW_LANDING_GLIDE = 45, + NAV_PERSISTENT_ID_FW_LANDING_FLARE = 46, + NAV_PERSISTENT_ID_FW_LANDING_ABORT = 47, } navigationPersistentId_e; typedef enum { @@ -279,6 +287,13 @@ typedef enum { NAV_STATE_CRUISE_INITIALIZE, NAV_STATE_CRUISE_IN_PROGRESS, NAV_STATE_CRUISE_ADJUSTING, + + NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, + NAV_STATE_FW_LANDING_LOITER, + NAV_STATE_FW_LANDING_APPROACH, + NAV_STATE_FW_LANDING_GLIDE, + NAV_STATE_FW_LANDING_FLARE, + NAV_STATE_FW_LANDING_ABORT, NAV_STATE_MIXERAT_INITIALIZE, NAV_STATE_MIXERAT_IN_PROGRESS, @@ -368,6 +383,12 @@ typedef struct { bool isApplied; // whether the safehome has been applied to home } safehomeState_t; +typedef enum { + FW_AUTOLAND_WP_TURN, + FW_AUTOLAND_WP_FINAL_APPROACH, + FW_AUTOLAND_WP_LAND, + FW_AUTOLAND_WP_COUNT, +} fwAutolandWayppoints_t; typedef struct { /* Flags and navigation system state */ navigationFSMState_t navState; @@ -434,6 +455,17 @@ typedef struct { int8_t activeRthTBPointIndex; int8_t rthTBWrapAroundCounter; // stores trackpoint array overwrite index position + /* Fixedwing autoland */ + timeUs_t fwLandLoiterStartTime; + fpVector3_t fwLandWaypoint[FW_AUTOLAND_WP_COUNT]; + int32_t fwLandingDirection; + int32_t fwLandAproachAltAgl; + int32_t fwLandAltAgl; + bool fwLandWpReached; + fwAutolandWayppoints_t fwLandCurrentWp; + bool fwLandAborted; + fwAutolandState_t fwLandState; + /* Internals & statistics */ int16_t rcAdjustment[4]; float totalTripDistance; diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index f0e54d9013..304168e9fb 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -810,6 +810,10 @@ static int logicConditionGetFlightOperandValue(int operand) { return rangefinderGetLatestRawAltitude(); break; + case LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE: + return posControl.fwLandState; + break; + default: return 0; break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index bb62881b78..5d45e44756 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -139,6 +139,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE, //int // 38 LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE, //0,1 // 39 LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW, // deg // 40 + LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE, // 41 } logicFlightOperands_e; typedef enum { diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c index b4e5b9283a..7a303cd50b 100644 --- a/src/main/target/SITL/sim/realFlight.c +++ b/src/main/target/SITL/sim/realFlight.c @@ -58,9 +58,10 @@ #define RF_PORT 18083 #define RF_MAX_CHANNEL_COUNT 12 -// RealFlight scenerys doesn't represent real landscapes, so fake some nice coords: Area 51 ;) -#define FAKE_LAT 37.277127f -#define FAKE_LON -115.799669f +// RealFlight scenerys doesn't represent real landscapes, so fake some nice coords +#define FAKE_LAT 0.0f +#define FAKE_LON 0.0f + static uint8_t pwmMapping[RF_MAX_PWM_OUTS]; static uint8_t mappingCount; diff --git a/src/main/target/common.h b/src/main/target/common.h index d2d135cfa2..bd3f58bd4f 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -142,6 +142,7 @@ #define NAV_FIXED_WING_LANDING #define USE_SAFE_HOME +#define USE_FW_AUTOLAND #define USE_AUTOTUNE_FIXED_WING #define USE_LOG #define USE_STATS