diff --git a/docs/Settings.md b/docs/Settings.md index 0d8eeb7f7e..1d1014a093 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -432,6 +432,7 @@ | osd_snr_alarm | 4 | -20 | 10 | Value below which Crossfire SNR Alarm pops-up. (dB) | | osd_stats_energy_unit | MAH | | | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | | osd_stats_min_voltage_unit | BATTERY | | | Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats. | +| osd_telemetry | OFF | | | To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST` | | osd_temp_label_align | LEFT | | | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` | | osd_time_alarm | 10 | 0 | 600 | Value above which to make the OSD flight time indicator blink (minutes) | | osd_units | METRIC | | | IMPERIAL, METRIC, UK | @@ -500,6 +501,9 @@ | smartport_fuel_unit | MAH | | | S.Port telemetry only: Unit of the value sent with the `FUEL` ID (FrSky D-Series always sends percent). [PERCENT/MAH/MWH] | | smartport_master_halfduplex | ON | | | | | smartport_master_inverted | OFF | | | | +| smith_predictor_delay | 0 | 0 | 8 | Expected delay of the gyro signal. In milliseconds | +| smith_predictor_lpf_hz | 50 | 1 | 500 | Cutoff frequency for the Smith Predictor Low Pass Filter | +| smith_predictor_strength | 0.5 | 0 | 1 | The strength factor of a Smith Predictor of PID measurement. In percents | | spektrum_sat_bind | `SPEKTRUM_SAT_BIND_DISABLED` | SPEKTRUM_SAT_BIND_DISABLED | SPEKTRUM_SAT_BIND_MAX | 0 = disabled. Used to bind the spektrum satellite to RX | | srxl2_baud_fast | ON | | | | | srxl2_unit_id | 1 | 0 | 15 | | diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index a827532ef2..28453ab2df 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -320,6 +320,8 @@ main_sources(COMMON_SRC flight/imu.h flight/kalman.c flight/kalman.h + flight/smith_predictor.c + flight/smith_predictor.h flight/mixer.c flight/mixer.h flight/pid.c diff --git a/src/main/build/debug.h b/src/main/build/debug.h index dfdb4c9533..f62d57547e 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -84,5 +84,6 @@ typedef enum { DEBUG_IMU2, DEBUG_ALTITUDE, DEBUG_GYRO_ALPHA_BETA_GAMMA, + DEBUG_SMITH_PREDICTOR, DEBUG_COUNT } debugType_e; diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 0941f48767..b620473ca7 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -119,8 +119,9 @@ #define SYM_RPM 0x8B // 139 RPM #define SYM_WAYPOINT 0x8C // 140 Waypoint #define SYM_AZIMUTH 0x8D // 141 Azimuth -// 0x8E // 142 - -// 0x8F // 143 - + +#define SYM_TELEMETRY_0 0x8E // 142 Antenna tracking telemetry +#define SYM_TELEMETRY_1 0x8F // 143 Antenna tracking telemetry #define SYM_BATT_FULL 0x90 // 144 Battery full #define SYM_BATT_5 0x91 // 145 Battery diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 34a9f9c9e3..c050d5e8b8 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -75,6 +75,9 @@ tables: - name: osd_video_system values: ["AUTO", "PAL", "NTSC"] enum: videoSystem_e + - name: osd_telemetry + values: ["OFF", "ON","TEST"] + enum: osd_telemetry_e - name: osd_alignment values: ["LEFT", "RIGHT"] enum: osd_alignment_e @@ -91,7 +94,7 @@ tables: "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE", - "DEBUG_GYRO_ALPHA_BETA_GAMMA"] + "GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator @@ -2080,6 +2083,27 @@ groups: field: fixedWingLevelTrim min: -10 max: 10 + - name: smith_predictor_strength + description: "The strength factor of a Smith Predictor of PID measurement. In percents" + default_value: 0.5 + field: smithPredictorStrength + condition: USE_SMITH_PREDICTOR + min: 0 + max: 1 + - name: smith_predictor_delay + description: "Expected delay of the gyro signal. In milliseconds" + default_value: 0 + field: smithPredictorDelay + condition: USE_SMITH_PREDICTOR + min: 0 + max: 8 + - name: smith_predictor_lpf_hz + description: "Cutoff frequency for the Smith Predictor Low Pass Filter" + default_value: 50 + field: smithPredictorFilterHz + condition: USE_SMITH_PREDICTOR + min: 1 + max: 500 - name: PG_PID_AUTOTUNE_CONFIG type: pidAutotuneConfig_t @@ -2922,6 +2946,12 @@ groups: headers: ["io/osd.h", "drivers/osd.h"] condition: USE_OSD members: + - name: osd_telemetry + description: "To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST`" + table: osd_telemetry + field: telemetry + type: uint8_t + default_value: "OFF" - name: osd_video_system description: "Video system used. Possible values are `AUTO`, `PAL` and `NTSC`" default_value: "AUTO" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b8c4b94e24..2541f21a58 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -48,6 +48,7 @@ FILE_COMPILE_FOR_SPEED #include "flight/rpm_filter.h" #include "flight/secondary_imu.h" #include "flight/kalman.h" +#include "flight/smith_predictor.h" #include "io/gps.h" @@ -109,6 +110,8 @@ typedef struct { bool itermFreezeActive; biquadFilter_t rateTargetFilter; + + smithPredictor_t smithPredictor; } pidState_t; STATIC_FASTRAM bool pidFiltersConfigured = false; @@ -159,7 +162,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; static EXTENDED_FASTRAM float fixedWingLevelTrim; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 2); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -297,6 +300,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, #endif .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT, + +#ifdef USE_SMITH_PREDICTOR + .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT, + .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT, + .smithPredictorFilterHz = SETTING_SMITH_PREDICTOR_LPF_HZ_DEFAULT, +#endif ); bool pidInitFilters(void) @@ -349,6 +358,30 @@ bool pidInitFilters(void) } } +#ifdef USE_SMITH_PREDICTOR + smithPredictorInit( + &pidState[FD_ROLL].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, + getLooptime() + ); + smithPredictorInit( + &pidState[FD_PITCH].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, + getLooptime() + ); + smithPredictorInit( + &pidState[FD_YAW].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, + getLooptime() + ); +#endif + pidFiltersConfigured = true; return true; @@ -1052,6 +1085,13 @@ void FAST_CODE pidController(float dT) pidState[axis].gyroRate = gyroKalmanUpdate(axis, pidState[axis].gyroRate, pidState[axis].rateTarget); } #endif + +#ifdef USE_SMITH_PREDICTOR + DEBUG_SET(DEBUG_SMITH_PREDICTOR, axis, pidState[axis].gyroRate); + pidState[axis].gyroRate = applySmithPredictor(axis, &pidState[axis].smithPredictor, pidState[axis].gyroRate); + DEBUG_SET(DEBUG_SMITH_PREDICTOR, axis + 3, pidState[axis].gyroRate); +#endif + DEBUG_SET(DEBUG_PID_MEASUREMENT, axis, pidState[axis].gyroRate); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3f3d5a3bcc..6162e3f31c 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -161,6 +161,11 @@ typedef struct pidProfile_s { #endif float fixedWingLevelTrim; +#ifdef USE_SMITH_PREDICTOR + float smithPredictorStrength; + float smithPredictorDelay; + uint16_t smithPredictorFilterHz; +#endif } pidProfile_t; typedef struct pidAutotuneConfig_s { diff --git a/src/main/flight/smith_predictor.c b/src/main/flight/smith_predictor.c new file mode 100644 index 0000000000..271c088add --- /dev/null +++ b/src/main/flight/smith_predictor.c @@ -0,0 +1,70 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it 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. + * + * This file is distributed in the hope that it 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 program. If not, see http://www.gnu.org/licenses/. + * + * This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight + * + */ + +#include "platform.h" +#ifdef USE_SMITH_PREDICTOR + +#include +#include "common/axis.h" +#include "common/utils.h" +#include "flight/smith_predictor.h" +#include "build/debug.h" + +FUNCTION_COMPILE_FOR_SPEED +float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) { + UNUSED(axis); + if (predictor->enabled) { + predictor->data[predictor->idx] = sample; + + predictor->idx++; + if (predictor->idx > predictor->samples) { + predictor->idx = 0; + } + + // filter the delayed data to help reduce the overall noise this prediction adds + float delayed = pt1FilterApply(&predictor->smithPredictorFilter, predictor->data[predictor->idx]); + float delayCompensatedSample = predictor->smithPredictorStrength * (sample - delayed); + + sample += delayCompensatedSample; + } + return sample; +} + +FUNCTION_COMPILE_FOR_SIZE +void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime) { + if (delay > 0.1) { + predictor->enabled = true; + predictor->samples = (delay * 1000) / looptime; + predictor->idx = 0; + predictor->smithPredictorStrength = strength; + pt1FilterInit(&predictor->smithPredictorFilter, filterLpfHz, looptime * 1e-6f); + } else { + predictor->enabled = false; + } +} + +#endif \ No newline at end of file diff --git a/src/main/flight/smith_predictor.h b/src/main/flight/smith_predictor.h new file mode 100644 index 0000000000..1d8ebcb13b --- /dev/null +++ b/src/main/flight/smith_predictor.h @@ -0,0 +1,45 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it 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. + * + * This file is distributed in the hope that it 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 program. If not, see http://www.gnu.org/licenses/. + * + * This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight + * + */ + +#pragma once + +#include +#include "common/filter.h" + +#define MAX_SMITH_SAMPLES 64 + +typedef struct smithPredictor_s { + bool enabled; + uint8_t samples; + uint8_t idx; + float data[MAX_SMITH_SAMPLES + 1]; + pt1Filter_t smithPredictorFilter; + float smithPredictorStrength; +} smithPredictor_t; + +float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample); +void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime); \ No newline at end of file diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7302ee568f..7f9d038753 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1183,6 +1183,67 @@ static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale) osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale); } +static uint16_t crc_accumulate(uint8_t data, uint16_t crcAccum) +{ + uint8_t tmp; + tmp = data ^ (uint8_t)(crcAccum & 0xff); + tmp ^= (tmp << 4); + crcAccum = (crcAccum >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4); + return crcAccum; +} + + +static void osdDisplayTelemetry(void) +{ + uint32_t trk_data; + uint16_t trk_crc = 0; + char trk_buffer[31]; + static int16_t trk_elevation = 127; + static uint16_t trk_bearing = 0; + + if (ARMING_FLAG(ARMED)) { + if (STATE(GPS_FIX)){ + if (GPS_distanceToHome > 5) { + trk_bearing = GPS_directionToHome; + trk_bearing += 360 + 180; + trk_bearing %= 360; + int32_t alt = CENTIMETERS_TO_METERS(osdGetAltitude()); + float at = atan2(alt, GPS_distanceToHome); + trk_elevation = (float)at * 57.2957795; // 57.2957795 = 1 rad + trk_elevation += 37; // because elevation in telemetry should be from -37 to 90 + if (trk_elevation < 0) { + trk_elevation = 0; + } + } + } + } + else{ + trk_elevation = 127; + trk_bearing = 0; + } + + trk_data = 0; // bit 0 - packet type 0 = bearing/elevation, 1 = 2 byte data packet + trk_data = trk_data | (uint32_t)(0x7F & trk_elevation) << 1; // bits 1-7 - elevation angle to target. NOTE number is abused. constrained value of -37 to 90 sent as 0 to 127. + trk_data = trk_data | (uint32_t)trk_bearing << 8; // bits 8-17 - bearing angle to target. 0 = true north. 0 to 360 + trk_crc = crc_accumulate(0xFF & trk_data, trk_crc); // CRC First Byte bits 0-7 + trk_crc = crc_accumulate(0xFF & trk_bearing, trk_crc); // CRC Second Byte bits 8-15 + trk_crc = crc_accumulate(trk_bearing >> 8, trk_crc); // CRC Third Byte bits 16-17 + trk_data = trk_data | (uint32_t)trk_crc << 17; // bits 18-29 CRC & 0x3FFFF + + for (uint8_t t_ctr = 0; t_ctr < 30; t_ctr++) { // Prepare screen buffer and write data line. + if (trk_data & (uint32_t)1 << t_ctr){ + trk_buffer[29 - t_ctr] = SYM_TELEMETRY_0; + } + else{ + trk_buffer[29 - t_ctr] = SYM_TELEMETRY_1; + } + } + trk_buffer[30] = 0; + displayWrite(osdDisplayPort, 0, 0, trk_buffer); + if (osdConfig()->telemetry>1){ + displayWrite(osdDisplayPort, 0, 3, trk_buffer); // Test display because normal telemetry line is not visible + } +} #endif static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) { @@ -1710,7 +1771,7 @@ static bool osdDrawSingleElement(uint8_t item) else if (FLIGHT_MODE(MANUAL_MODE)) p = "MANU"; else if (FLIGHT_MODE(NAV_RTH_MODE)) - p = "RTH "; + p = isWaypointMissionRTHActive() ? "WRTH" : "RTH "; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) p = "HOLD"; else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) @@ -2691,8 +2752,11 @@ void osdDrawNextElement(void) elementIndex = osdIncElementIndex(elementIndex); } while(!osdDrawSingleElement(elementIndex) && index != elementIndex); - // Draw artificial horizon last + // Draw artificial horizon + tracking telemtry last osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + if (osdConfig()->telemetry>0){ + osdDisplayTelemetry(); + } } PG_RESET_TEMPLATE(osdConfig_t, osdConfig, @@ -3612,6 +3676,11 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } else { if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { + 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); + } + if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { // Countdown display for remaining Waypoints tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount); diff --git a/src/main/io/osd.h b/src/main/io/osd.h old mode 100755 new mode 100644 index b32722ce6a..c9ad2eaea2 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -87,6 +87,7 @@ #define OSD_MSG_HOLDING_WAYPOINT "HOLDING WAYPOINT" #define OSD_MSG_TO_WP "TO WP" #define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT" +#define OSD_MSG_WP_RTH_CANCEL "CANCEL WP MODE TO EXIT RTH" #define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING" #define OSD_MSG_LANDING "LANDING" #define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME" @@ -368,6 +369,7 @@ typedef struct osdConfig_s { uint8_t pan_servo_index; // Index of the pan servo used for home direction offset int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm uint8_t crsf_lq_format; + uint8_t telemetry; // use telemetry on displayed pixel line 0 } osdConfig_t; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c old mode 100755 new mode 100644 index dd89b8b69e..94db2be867 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -238,6 +238,7 @@ void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distanc static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint); static navigationFSMEvent_t nextForNonGeoStates(void); +static bool isWaypointMissionValid(void); void initializeRTHSanityChecker(const fpVector3_t * pos); bool validateRTHSanityChecker(void); @@ -273,7 +274,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState); @@ -665,11 +665,12 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP) - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP) + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, } }, @@ -703,19 +704,18 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME] = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -794,27 +794,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, - [NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME] = { - .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME, - .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, - .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_WP, - .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, - .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME, - .mwError = MW_NAV_ERROR_NONE, - .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - } - }, - /** EMERGENCY LANDING ************************************************/ [NAV_STATE_EMERGENCY_LANDING_INITIALIZE] = { .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE, @@ -1331,30 +1310,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND // If position ok OR within valid timeout - continue if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) { - - // Wait until target heading is reached (with 15 deg margin for error) - if (STATE(FIXED_WING_LEGACY)) { + // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing + if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) { resetLandingDetector(); updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); - return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; + return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land + } + else if (!validateRTHSanityChecker()) { + // Continue to check for RTH sanity during pre-landing hover + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } else { - if (ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { - resetLandingDetector(); - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); - return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; - } - else if (!validateRTHSanityChecker()) { - // Continue to check for RTH sanity during pre-landing hover - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } - else { - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); - setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - return NAV_FSM_EVENT_NONE; - } + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return NAV_FSM_EVENT_NONE; } - } else { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1549,10 +1519,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav return nextForNonGeoStates(); case NAV_WP_ACTION_RTH: - posControl.rthState.rthInitialDistance = posControl.homeDistance; - initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); - calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL)); - return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS + return NAV_FSM_EVENT_SWITCH_TO_RTH; }; UNREACHABLE(); @@ -1598,21 +1565,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na case NAV_WP_ACTION_JUMP: case NAV_WP_ACTION_SET_HEAD: case NAV_WP_ACTION_SET_POI: - UNREACHABLE(); - case NAV_WP_ACTION_RTH: - if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) { - return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED - } - else { - if(navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) - setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING_TAIL_FIRST); - else - setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); - setDesiredPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL), 0, NAV_POS_UPDATE_Z); - return NAV_FSM_EVENT_NONE; // will re-process state in >10ms - } - break; + UNREACHABLE(); } } /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ @@ -1637,15 +1591,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga case NAV_WP_ACTION_JUMP: case NAV_WP_ACTION_SET_HEAD: case NAV_WP_ACTION_SET_POI: - UNREACHABLE(); - case NAV_WP_ACTION_RTH: - if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) { - return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND; - } - else { - return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME; - } + UNREACHABLE(); case NAV_WP_ACTION_LAND: return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND; @@ -1725,14 +1672,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig } } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME(navigationFSMState_t previousState) -{ - UNUSED(previousState); - - const navigationFSMEvent_t hoverEvent = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(previousState); - return hoverEvent; -} - static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState) { // TODO: @@ -3656,6 +3595,11 @@ rthState_e getStateOfForcedRTH(void) } } +bool isWaypointMissionRTHActive(void) +{ + return FLIGHT_MODE(NAV_RTH_MODE) && IS_RC_MODE_ACTIVE(BOXNAVWP) && !(IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated); +} + bool navigationIsExecutingAnEmergencyLanding(void) { return navGetCurrentStateFlags() & NAV_CTL_EMERG; @@ -3681,9 +3625,12 @@ bool navigationIsFlyingAutonomousMode(void) bool navigationRTHAllowsLanding(void) { - if (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND) - return true; + // WP mission RTH landing setting + if (isWaypointMissionRTHActive() && isWaypointMissionValid()) { + return posControl.waypointList[posControl.waypointCount - 1].p1 > 0; + } + // normal RTH landing setting navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing; return allow == NAV_RTH_ALLOW_LANDING_ALWAYS || (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE)); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 863f07a4bf..0638e6e844 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -520,6 +520,7 @@ bool navigationIsExecutingAnEmergencyLanding(void); * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active. */ bool navigationRTHAllowsLanding(void); +bool isWaypointMissionRTHActive(void); bool isNavLaunchEnabled(void); bool isFixedWingLaunchDetected(void); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index d20bd76d85..b26299e707 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -141,7 +141,6 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3, - NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME, NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD, NAV_FSM_EVENT_SWITCH_TO_CRUISE, @@ -200,7 +199,7 @@ typedef enum { NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35, NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36, - NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME = 37, + NAV_PERSISTENT_ID_UNUSED_4 = 37, // was NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME } navigationPersistentId_e; @@ -232,7 +231,6 @@ typedef enum { NAV_STATE_WAYPOINT_NEXT, NAV_STATE_WAYPOINT_FINISHED, NAV_STATE_WAYPOINT_RTH_LAND, - NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, NAV_STATE_EMERGENCY_LANDING_INITIALIZE, NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, diff --git a/src/main/target/common.h b/src/main/target/common.h index 7692dbfa15..7fba9cd1a1 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -87,6 +87,7 @@ #define USE_ALPHA_BETA_GAMMA_FILTER #define USE_DYNAMIC_FILTERS #define USE_GYRO_KALMAN +#define USE_SMITH_PREDICTOR #define USE_EXTENDED_CMS_MENUS #define USE_HOTT_TEXTMODE