diff --git a/docs/Settings.md b/docs/Settings.md index cbc526adb33..45298ffd26a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1356,6 +1356,56 @@ S.Port telemetry: If `ON`, send the legacy telemetry IDs for modes (Tmp1) and GN --- +### fw_auto_speed_channel + +Channel number used to set desired Auto Speed demand value. Defaults to throttle channel 4. + +| Default | Min | Max | +| --- | --- | --- | +| 4 | 4 | MAX_SUPPORTED_RC_CHANNEL_COUNT | + +--- + +### fw_auto_speed_level_min_thr + +Minimum throttle for LEVEL flight in auto speed mode when using ground speed as speed reference [us]. This should be set with just enough margin to avoid stalling. Throttle can go lower than this setting when pitching down to a minimum of nav_fw_min_thr. + +| Default | Min | Max | +| --- | --- | --- | +| 1300 | PWM_RANGE_MIN | PWM_RANGE_MAX | + +--- + +### fw_auto_speed_max_speed + +Maximum allowed speed demand for Auto Speed mode [m/s]. + +| Default | Min | Max | +| --- | --- | --- | +| 22 | 5 | 50 | + +--- + +### fw_auto_speed_min_speed + +Minimum allowed speed demand for Auto Speed mode [m/s]. + +| Default | Min | Max | +| --- | --- | --- | +| 11 | 5 | 50 | + +--- + +### fw_auto_speed_thr_smoothing + +Changes how smoothly the throttle responds in Auto Speed mode. Increasing the setting makes throttle changes smoother but also reduces throttle response. + +| Default | Min | Max | +| --- | --- | --- | +| 4 | 1 | 10 | + +--- + ### fw_autotune_max_rate_deflection The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`. @@ -3417,6 +3467,36 @@ Maximum climb/descent rate that UAV is allowed to reach during navigation modes. --- +### nav_fw_auto_speed_d + +D gain of auto speed PID controller. + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 255 | + +--- + +### nav_fw_auto_speed_i + +I gain of auto speed PID controller. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 255 | + +--- + +### nav_fw_auto_speed_p + +P gain of auto speed PID controller. + +| Default | Min | Max | +| --- | --- | --- | +| 30 | 0 | 255 | + +--- + ### nav_fw_bank_angle Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e5cfe7e81e8..454bc7fe4e5 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -166,7 +166,9 @@ static const char pidnames[] = "NavR;" "LEVEL;" "MAG;" - "VEL;"; + "VEL;" + "HEADING;" + "SPEED;"; typedef enum { MSP_SDCARD_STATE_NOT_PRESENT = 0, @@ -1772,7 +1774,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, timer2id(timerHardware[i].tim)); #endif sbufWriteU32(dst, timerHardware[i].usageFlags); - + #if defined(SITL_BUILD) || defined(WASM_BUILD) sbufWriteU8(dst, 0); #else @@ -3054,7 +3056,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) osdDrawCustomItem(item); return MSP_RESULT_ACK; - + } else{ return MSP_RESULT_ERROR; } @@ -4410,7 +4412,7 @@ static void readMspSimulatorValues(sbuf_t *src, const int dataSize, const uint8_ } // Feed data to navigation gpsProcessNewDriverData(); - gpsProcessNewSolutionData(false); + gpsProcessNewSolutionData(false); } else { sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); } @@ -4463,26 +4465,26 @@ static void readMspSimulatorValues(sbuf_t *src, const int dataSize, const uint8_ sbufReadU16(src); } - if (simMspVersion == SIMULATOR_MSP_VERSION_3) { - + if (simMspVersion == SIMULATOR_MSP_VERSION_3) { + if (SIMULATOR_HAS_OPTION(HITL_RANGEFINDER)) { simulatorData.rangefinder = sbufReadU16(src); if (simulatorData.rangefinder == 0xFFFF) { fakeRangefindersSetData(-1); } else { - fakeRangefindersSetData(simulatorData.rangefinder); + fakeRangefindersSetData(simulatorData.rangefinder); } - + } else { sbufReadU16(src); } - + if (SIMULATOR_HAS_OPTION(HITL_CURRENT_SENSOR)) { simulatorData.current = sbufReadU16(src); } else { sbufReadU16(src); } - + if (SIMULATOR_HAS_OPTION(HITL_SIM_RC_INPUT)) { for (int i = 0; i < HITL_SIM_MAX_RC_INPUTS; i++) { simulatorData.rcInput[i] = sbufReadU16(src); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 65654ccd97b..842b56563a0 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -109,6 +109,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXGIMBALRLOCK, .boxName = "GIMBAL LEVEL ROLL", .permanentId = 66 }, { .boxId = BOXGIMBALCENTER, .boxName = "GIMBAL CENTER", .permanentId = 67 }, { .boxId = BOXGIMBALHTRK, .boxName = "GIMBAL HEADTRACKER", .permanentId = 68 }, + { .boxId = BOXAUTOSPEED, .boxName = "AUTO SPEED", .permanentId = 69 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -248,6 +249,7 @@ void initActiveBoxIds(void) if (STATE(AIRPLANE) || platformTypeConfigured(PLATFORM_AIRPLANE)) { ADD_ACTIVE_BOX(BOXSOARING); + ADD_ACTIVE_BOX(BOXAUTOSPEED); } } @@ -449,7 +451,6 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION); #endif CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)), BOXANGLEHOLD); - #ifdef USE_SERIAL_GIMBAL if(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) { CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALCENTER); @@ -463,6 +464,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALRLOCK); } #endif + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOSPEED)), BOXAUTOSPEED); memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 2e972d1b304..48e4357a1a5 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -85,6 +85,7 @@ typedef enum { BOXGIMBALRLOCK = 57, BOXGIMBALCENTER = 58, BOXGIMBALHTRK = 59, + BOXAUTOSPEED = 60, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 15a0c0fff4f..27d71c415b8 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1177,6 +1177,12 @@ groups: field: nav.fw.launch_idle_throttle min: 1000 max: 2000 + - name: fw_auto_speed_level_min_thr + description: "Minimum throttle for LEVEL flight in auto speed mode when using ground speed as speed reference [us]. This should be set with just enough margin to avoid stalling. Throttle can go lower than this setting when pitching down to a minimum of nav_fw_min_thr." + default_value: 1300 + field: nav.fw.auto_speed_level_min_thr + min: PWM_RANGE_MIN + max: PWM_RANGE_MAX - name: limit_cont_current description: "Continous current limit (dA), set to 0 to disable" condition: USE_POWER_LIMITS @@ -2263,6 +2269,24 @@ groups: field: navFwPosHdgPidsumLimit min: PID_SUM_LIMIT_MIN max: PID_SUM_LIMIT_MAX + - name: nav_fw_auto_speed_p + description: "P gain of auto speed PID controller." + default_value: 30 + field: bank_fw.pid[PID_AUTO_SPEED].P + min: 0 + max: 255 + - name: nav_fw_auto_speed_i + description: "I gain of auto speed PID controller." + default_value: 5 + field: bank_fw.pid[PID_AUTO_SPEED].I + min: 0 + max: 255 + - name: nav_fw_auto_speed_d + description: "D gain of auto speed PID controller." + default_value: 10 + field: bank_fw.pid[PID_AUTO_SPEED].D + min: 0 + max: 255 - name: mc_iterm_relax description: "Iterm relax type. When enabled, Iterm will be relaxed when stick is centered. This will help to reduce bounceback and followthrough on multirotors. It is recommended to enable this feature on all multirotors." field: iterm_relax @@ -3001,6 +3025,30 @@ groups: field: fw.cruise_speed min: 0 max: 65535 + - name: fw_auto_speed_min_speed + description: "Minimum allowed speed demand for Auto Speed mode [m/s]." + default_value: 11 + field: fw.auto_speed_min_speed + min: 5 + max: 50 + - name: fw_auto_speed_max_speed + description: "Maximum allowed speed demand for Auto Speed mode [m/s]." + default_value: 22 + field: fw.auto_speed_max_speed + min: 5 + max: 50 + - name: fw_auto_speed_channel + description: "Channel number used to set desired Auto Speed demand value. Defaults to throttle channel 4." + default_value: 4 + field: fw.auto_speed_channel + min: 4 + max: MAX_SUPPORTED_RC_CHANNEL_COUNT + - name: fw_auto_speed_thr_smoothing + description: "Changes how smoothly the throttle responds in Auto Speed mode. Increasing the setting makes throttle changes smoother but also reduces throttle response." + default_value: 4 + field: fw.auto_speed_thr_smoothing + min: 1 + max: 10 - name: nav_fw_control_smoothness description: "How smoothly the autopilot controls the airplane to correct the navigation error" default_value: 0 diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 60eb964bd68..600a10afb24 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -32,9 +32,9 @@ typedef union { int16_t raw[XYZ_AXIS_COUNT]; struct { // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 - int16_t roll; - int16_t pitch; - int16_t yaw; + int16_t roll; // +ve = Roll right + int16_t pitch; // +ve = Pitch down ! + int16_t yaw; // +ve = Yaw right } values; } attitudeEulerAngles_t; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a80992b772d..27f1ec7ffe2 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -676,7 +676,7 @@ motorStatus_e getMotorStatus(void) const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE); - if (throttleStickIsLow() && fixedWingOrAirmodeNotActive) { + if (throttleStickIsLow() && fixedWingOrAirmodeNotActive && !isFixedwingAutoSpeedActive()) { if ((navConfig()->general.flags.nav_overrides_motor_stop == NOMS_OFF_ALWAYS) && failsafeIsActive()) { // If we are in failsafe and user was holding stick low before it was triggered and nav_overrides_motor_stop is set to OFF_ALWAYS // and either on a plane or on a quad with inactive airmode - stop motor @@ -688,7 +688,7 @@ motorStatus_e getMotorStatus(void) switch (navConfig()->general.flags.nav_overrides_motor_stop) { case NOMS_ALL_NAV: - return navigationInAutomaticThrottleMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER; + return navigationRequiresAutoThrottleMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER; case NOMS_AUTO_ONLY: return navigationIsFlyingAutonomousMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER; @@ -724,6 +724,11 @@ bool areMotorsRunning(void) return false; } +bool areMotorsStopped(void) +{ + return motor[0] == motorZeroCommand; +} + uint16_t getMaxThrottle(void) { static uint16_t throttle = 0; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 6c4370d4176..97b85d8941d 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -131,5 +131,6 @@ void stopPwmAllMotors(void); void loadPrimaryMotorMixer(void); bool areMotorsRunning(void); +bool areMotorsStopped(void); uint16_t getMaxThrottle(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a8168fa6081..bfdc890c602 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -224,6 +224,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .FF = 0, }, [PID_POS_HEADING] = { + .P = 0, + .I = 0, + .D = 0, + .FF = 0, + }, + [PID_AUTO_SPEED] = { .P = 0, .I = 0, .D = 0, @@ -260,6 +266,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .P = SETTING_NAV_FW_POS_HDG_P_DEFAULT, .I = SETTING_NAV_FW_POS_HDG_I_DEFAULT, .D = SETTING_NAV_FW_POS_HDG_D_DEFAULT, + .FF = 0, + }, + [PID_AUTO_SPEED] = { + .P = SETTING_NAV_FW_AUTO_SPEED_P_DEFAULT, + .I = SETTING_NAV_FW_AUTO_SPEED_I_DEFAULT, + .D = SETTING_NAV_FW_AUTO_SPEED_D_DEFAULT, .FF = 0 } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 68f09018763..cb428140cf6 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -68,6 +68,7 @@ typedef enum { PID_HEADING, // + + PID_VEL_Z, // + n/a PID_POS_HEADING,// n/a + + PID_AUTO_SPEED, // n/a + PID_ITEM_COUNT } pidIndex_e; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 67416a16dff..efcbb72aa7b 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1165,7 +1165,7 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes #endif int8_t throttlePercent = getThrottlePercent(useScaled); if ((useScaled && throttlePercent <= 0) || !ARMING_FLAG(ARMED)) { - const char* message = ARMING_FLAG(ARMED) ? (throttlePercent == 0 && !ifMotorstopFeatureEnabled()) ? "IDLE" : "STOP" : "DARM"; + const char* message = ARMING_FLAG(ARMED) ? areMotorsStopped() ? "STOP" : "IDLE" : "DARM"; buff[0] = SYM_THR; strcpy(buff + 1, message); return; @@ -1991,6 +1991,23 @@ static bool osdDrawSingleElement(uint8_t item) osdFormatVelocityStr(buff, stats.max_3D_speed, OSD_SPEED_TYPE_3D, true); break; + case OSD_AUTO_SPEED: + if (IS_RC_MODE_ACTIVE(BOXAUTOSPEED)) { + buff[0] = posControl.autoSpeedSpdSource == FW_AUTO_SPD_GROUND ? 'G' : 'A'; + strcpy(buff + 1, ": OFF"); + if (isFixedwingAutoSpeedActive()) { + osdFormatVelocityStr(buff + 2, posControl.desiredState.autoSpeedDemand, OSD_SPEED_TYPE_3D, false); + buff[6] = '\0'; + if (posControl.autoSpeedSpdSource == FW_AUTO_SPD_GROUND_OVERRIDE && OSD_ALTERNATING_CHOICES(1000, 2) == 0) { + buff[0] = 'G'; + } + } + break; + } else { + displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); + return true; + } + case OSD_GLIDESLOPE: { float horizontalSpeed = gpsSol.groundSpeed; @@ -4362,6 +4379,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1); osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1); osdLayoutsConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1); + osdLayoutsConfig->item_pos[0][OSD_AUTO_SPEED] = OSD_POS(23, 0); osdLayoutsConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1); osdLayoutsConfig->item_pos[0][OSD_GLIDESLOPE] = OSD_POS(23, 2); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 4767ae035d6..21af2d7b290 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -342,7 +342,8 @@ typedef enum { OSD_NAV_FW_ALT_CONTROL_RESPONSE, OSD_NAV_MIN_GROUND_SPEED, OSD_THROTTLE_GAUGE, - OSD_GPS_EXTRA_STATS, + OSD_GPS_EXTRA_STATS, + OSD_AUTO_SPEED, // 170 OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 5b4f38e686c..5701d3410dd 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -119,7 +119,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 8); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -211,6 +211,10 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s + .auto_speed_min_speed = SETTING_FW_AUTO_SPEED_MIN_SPEED_DEFAULT, // 11 m/s + .auto_speed_max_speed = SETTING_FW_AUTO_SPEED_MAX_SPEED_DEFAULT, // 22 m/s + .auto_speed_channel = SETTING_FW_AUTO_SPEED_CHANNEL_DEFAULT, // 4 + .auto_speed_thr_smoothing = SETTING_FW_AUTO_SPEED_THR_SMOOTHING_DEFAULT, // 4 .control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT, .pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT, .pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT, @@ -406,7 +410,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT | NAV_RC_ALT, + .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_CTL_SPEED, .mapToFlightModes = NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, @@ -442,7 +446,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW | NAV_CTL_SPEED, .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE, .mwState = MW_NAV_STATE_HOLD_INFINIT, .mwError = MW_NAV_ERROR_NONE, @@ -478,7 +482,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_RC_POS | NAV_RC_YAW, + .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_RC_POS | NAV_RC_YAW | NAV_CTL_SPEED, .mapToFlightModes = NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, @@ -500,7 +504,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING, .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING, .timeoutMs = 10, - .stateFlags = NAV_REQUIRE_ANGLE | NAV_RC_POS, + .stateFlags = NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_CTL_SPEED, .mapToFlightModes = NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, @@ -539,7 +543,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT | NAV_CTL_SPEED, .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, @@ -561,7 +565,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_CRUISE_ADJUSTING, .onEntry = navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_ALT, + .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_ALT | NAV_CTL_SPEED, .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, @@ -603,7 +607,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT, .onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, // allow pos adjustment while climbing to safe alt + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_CTL_SPEED, // allow pos adjustment while climbing to safe alt .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_RTH_CLIMB, .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT, @@ -623,7 +627,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_TRACKBACK, .onEntry = navOnEnteringState_NAV_STATE_RTH_TRACKBACK, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_CTL_SPEED, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_RTH_ENROUTE, .mwError = MW_NAV_ERROR_NONE, @@ -643,7 +647,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_HEAD_HOME, .onEntry = navOnEnteringState_NAV_STATE_RTH_HEAD_HOME, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_CTL_SPEED, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_RTH_ENROUTE, .mwError = MW_NAV_ERROR_NONE, @@ -665,7 +669,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING, .onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, .timeoutMs = 500, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_CTL_SPEED, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LAND_SETTLE, .mwError = MW_NAV_ERROR_NONE, @@ -686,7 +690,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME, .onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT | NAV_CTL_SPEED, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME, .mwError = MW_NAV_ERROR_NONE, @@ -774,7 +778,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION, .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_CTL_SPEED, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, @@ -793,7 +797,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_CTL_SPEED, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_WP_ENROUTE, .mwError = MW_NAV_ERROR_NONE, @@ -815,7 +819,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_WAYPOINT_REACHED, .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_REACHED, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_CTL_SPEED, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, @@ -840,7 +844,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME, // There is no state for timed hold? .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_CTL_SPEED, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_HOLD_TIMED, .mwError = MW_NAV_ERROR_NONE, @@ -886,7 +890,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_WAYPOINT_NEXT, .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_NEXT, .timeoutMs = 0, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_CTL_SPEED, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, @@ -900,7 +904,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED, .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE | NAV_CTL_SPEED, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_WP_ENROUTE, .mwError = MW_NAV_ERROR_FINISH, @@ -1069,7 +1073,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .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_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH | NAV_CTL_SPEED, .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, @@ -1090,7 +1094,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .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_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH | NAV_CTL_SPEED, .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, @@ -1111,7 +1115,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .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_AUTO_WP, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_WP | NAV_CTL_SPEED, .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, @@ -3562,7 +3566,7 @@ void updateLandingStatus(timeMs_t currentTimeMs) if (navConfig()->general.flags.disarm_on_landing && !FLIGHT_MODE(FAILSAFE_MODE)) { ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); disarm(DISARM_LANDING); - } else if (!navigationInAutomaticThrottleMode()) { + } else if (!navigationRequiresAutoThrottleMode()) { if (STATE(AIRPLANE) && isFlightDetected()) { // Cancel landing detection flag if fixed wing redetected in flight resetLandingDetector(); @@ -3603,7 +3607,7 @@ bool isProbablyStillFlying(void) if (STATE(MULTIROTOR)) { inFlightSanityCheck = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING || averageAbsGyroRates() > 4.0f; } else { - inFlightSanityCheck = isGPSHeadingValid(); + inFlightSanityCheck = isGPSHeadingValid() || posControl.actualState.vel3D > 300; } return landingDetectorIsActive && inFlightSanityCheck; @@ -4411,6 +4415,7 @@ void applyWaypointNavigationAndAltitudeHold(void) applyRoverBoatNavigationController(navStateFlags, currentTimeUs); } else if (STATE(FIXED_WING_LEGACY)) { applyFixedWingNavigationController(navStateFlags, currentTimeUs); + applyAutoSpeedThrottleDemand(&rcCommand[THROTTLE], currentTimeUs); } else { applyMulticopterNavigationController(navStateFlags, currentTimeUs); @@ -4765,7 +4770,8 @@ bool navigationPositionEstimateIsHealthy(void) navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) { const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || - IS_RC_MODE_ACTIVE(BOXNAVCRUISE) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)); + IS_RC_MODE_ACTIVE(BOXNAVCRUISE) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || + (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXAUTOSPEED) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD))); if (usedBypass) { *usedBypass = false; @@ -5058,6 +5064,14 @@ void navigationUsePIDs(void) 2.0f, 0.0f ); + + navPidInit(&posControl.pids.fw_autoSpeed, (float)pidProfile()->bank_fw.pid[PID_AUTO_SPEED].P / 30.0f, + (float)pidProfile()->bank_fw.pid[PID_AUTO_SPEED].I / 50.0f, + (float)pidProfile()->bank_fw.pid[PID_AUTO_SPEED].D / 50.0f, + 0.0f, + 2.0f, + 0.0f + ); } void navigationInit(void) @@ -5253,7 +5267,7 @@ bool navigationIsExecutingAnEmergencyLanding(void) return navGetCurrentStateFlags() & NAV_CTL_EMERG; } -bool navigationInAutomaticThrottleMode(void) +bool navigationRequiresAutoThrottleMode(void) { navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAND)) || @@ -5262,8 +5276,10 @@ bool navigationInAutomaticThrottleMode(void) bool navigationIsControllingThrottle(void) { + if (isFixedwingAutoSpeedActive()) return true; + // Note that this makes a detour into mixer code to evaluate actual motor status - return navigationInAutomaticThrottleMode() && getMotorStatus() != MOTOR_STOPPED_USER && !FLIGHT_MODE(SOARING_MODE); + return navigationRequiresAutoThrottleMode() && getMotorStatus() != MOTOR_STOPPED_USER && !FLIGHT_MODE(SOARING_MODE); } bool navigationIsControllingAltitude(void) { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 398781fa596..71358e42937 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -197,7 +197,7 @@ typedef struct geozone_s { int32_t distanceHorToNearestZone; int32_t distanceVertToNearestZone; int32_t zoneInfo; - int32_t currentzoneMaxAltitude; + int32_t currentzoneMaxAltitude; int32_t currentzoneMinAltitude; bool nearestHorZoneHasAction; bool sticksLocked; @@ -473,6 +473,10 @@ typedef struct navConfig_s { uint8_t max_climb_angle; // Fixed wing max banking angle (deg) uint8_t max_dive_angle; // Fixed wing max banking angle (deg) uint16_t cruise_speed; // Speed at cruise throttle (cm/s), used for time/distance left before RTH + uint16_t auto_speed_min_speed; // Minimum allowed speed for auto speed mode (m/s) + uint16_t auto_speed_max_speed; // Maximum allowed speed for auto speed mode (m/s) + uint8_t auto_speed_channel; // Input channel number for auto speed mode + uint8_t auto_speed_thr_smoothing; // Throttle smoothing filtering adjustment factor uint8_t control_smoothness; // The amount of smoothing to apply to controls for navigation uint16_t pitch_to_throttle_smooth; // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. uint8_t pitch_to_throttle_thresh; // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] @@ -598,6 +602,7 @@ typedef struct navigationPIDControllers_s { pidController_t fw_alt; pidController_t fw_nav; pidController_t fw_heading; + pidController_t fw_autoSpeed; } navigationPIDControllers_t; /* MultiWii-compatible params for telemetry */ @@ -776,7 +781,7 @@ void abortForcedEmergLanding(void); emergLandState_e getStateOfForcedEmergLanding(void); /* Getter functions which return data about the state of the navigation system */ -bool navigationInAutomaticThrottleMode(void); +bool navigationRequiresAutoThrottleMode(void); bool navigationIsControllingThrottle(void); bool isFixedWingAutoThrottleManuallyIncreased(void); bool navigationIsFlyingAutonomousMode(void); @@ -819,6 +824,8 @@ bool rthAltControlStickOverrideCheck(uint8_t axis); int8_t navCheckActiveAngleHoldAxis(void); uint8_t getActiveWpNumber(void); uint16_t getFlownLoiterRadius(void); +bool isFixedwingAutoSpeedActive(void); +void applyAutoSpeedThrottleDemand(int16_t *throttleCommand, timeUs_t currentTimeUs); /* Returns the heading recorded when home position was acquired. * Note that the navigation system uses deg*100 as unit and angles diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 92ebb699f89..e73e30e95ca 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -608,35 +608,24 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) { static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate - // Apply controller only if position source is valid - if ((posControl.flags.estPosStatus >= EST_USABLE)) { - // If we have new position - update velocity and acceleration controllers - if (posControl.flags.horizontalPositionDataNew) { - const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; - previousTimePositionUpdate = currentTimeUs; + // If we have new position - update min speed controller + if (posControl.flags.horizontalPositionDataNew) { + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + previousTimePositionUpdate = currentTimeUs; - if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { - float velThrottleBoost = ((getMinGroundSpeed(navConfig()->general.min_ground_speed) * 100.0f) - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); - - // If we are in the deadband of 50cm/s - don't update speed boost - if (fabsf(posControl.actualState.velXY - (getMinGroundSpeed(navConfig()->general.min_ground_speed) * 100.0f)) > 50) { - throttleSpeedAdjustment += velThrottleBoost; - } + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { + float velThrottleBoost = ((getMinGroundSpeed(navConfig()->general.min_ground_speed) * 100.0f) - posControl.actualState.velXY) * + NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); - throttleSpeedAdjustment = constrainf(throttleSpeedAdjustment, 0.0f, 500.0f); - } - else { - // Position update has not occurred in time (first iteration or glitch), reset altitude controller - throttleSpeedAdjustment = 0; - } - - // Indicate that information is no longer usable - posControl.flags.horizontalPositionDataConsumed = true; + throttleSpeedAdjustment += velThrottleBoost; + throttleSpeedAdjustment = constrainf(throttleSpeedAdjustment, 0.0f, 500.0f); } - } - else { - // No valid pos sensor data, we can't calculate speed - throttleSpeedAdjustment = 0; + else { + // Reset if position update has not occurred in time (first iteration or glitch) + throttleSpeedAdjustment = 0; + } + // Indicate that information is no longer usable + posControl.flags.horizontalPositionDataConsumed = true; } return throttleSpeedAdjustment; @@ -672,12 +661,17 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs) { - int16_t minThrottleCorrection = currentBatteryProfile->nav.fw.min_throttle - currentBatteryProfile->nav.fw.cruise_throttle; - int16_t maxThrottleCorrection = currentBatteryProfile->nav.fw.max_throttle - currentBatteryProfile->nav.fw.cruise_throttle; + const uint16_t cruiseThrottle = currentBatteryProfile->nav.fw.cruise_throttle; + const uint16_t minThrottle = currentBatteryProfile->nav.fw.min_throttle; + const uint16_t maxThrottle = currentBatteryProfile->nav.fw.max_throttle; + + int16_t minThrottleCorrection = minThrottle - cruiseThrottle; + int16_t maxThrottleCorrection = maxThrottle - cruiseThrottle; if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) { // ROLL >0 right, <0 left - int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle)); + const uint8_t maxBankAngle = navConfig()->fw.max_bank_angle; + int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL], -DEGREES_TO_DECIDEGREES(maxBankAngle), DEGREES_TO_DECIDEGREES(maxBankAngle)); rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]); } @@ -689,36 +683,44 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat // PITCH >0 dive, <0 climb int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle)); rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]); - int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs); - if (navStateFlags & NAV_CTL_LAND) { - // During LAND we do not allow to raise THROTTLE when nose is up to reduce speed - throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0); + if (isFixedwingAutoSpeedActive()) { + isAutoThrottleManuallyIncreased = false; } else { - throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection); - } + int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs); - // Speed controller - only apply in POS mode when NOT NAV_CTL_LAND - if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) { - throttleCorrection += applyFixedWingMinSpeedController(currentTimeUs); - throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection); - } + if (navStateFlags & NAV_CTL_LAND) { + // During LAND we do not allow to raise THROTTLE when nose is up to reduce speed + throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0); + } else { + throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection); + } + + // Min Speed controller - only apply in POS mode when NOT NAV_CTL_LAND + if (posControl.flags.estPosStatus >= EST_USABLE && (navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) { + bool speedBoostRequired = (getMinGroundSpeed(navConfig()->general.min_ground_speed) * 100.0f - posControl.actualState.velXY > 0) && !throttleSpeedAdjustment; + if (speedBoostRequired || throttleSpeedAdjustment) { + throttleCorrection += applyFixedWingMinSpeedController(currentTimeUs); + throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection); + } + } - uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle); + uint16_t correctedThrottleValue = constrain(cruiseThrottle + throttleCorrection, minThrottle, maxThrottle); - // Manual throttle increase - if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { - if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){ - correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle); + // Manual throttle increase + if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { + if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){ + correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - cruiseThrottle); + } else { + correctedThrottleValue = getMaxThrottle(); + } + isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > cruiseThrottle); } else { - correctedThrottleValue = getMaxThrottle(); + isAutoThrottleManuallyIncreased = false; } - isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle); - } else { - isAutoThrottleManuallyIncreased = false; - } - rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false); + rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false); + } } #ifdef USE_FW_AUTOLAND @@ -870,6 +872,99 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs) } } +/*----------------------------------------------------------- + * Auto Speed mode control + *-----------------------------------------------------------*/ +bool isFixedwingAutoSpeedActive(void) +{ + bool thrStickEmergStop = navConfig()->fw.auto_speed_channel != (THROTTLE + 1) && throttleStickIsLow(); + + return STATE(AIRPLANE) && ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXAUTOSPEED) && isProbablyStillFlying() && !thrStickEmergStop && + !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) && + posControl.flags.estVelStatus == EST_TRUSTED && posControl.flags.estAltStatus == EST_TRUSTED && + !(navigationRequiresAutoThrottleMode() && !(navGetCurrentStateFlags() & NAV_CTL_SPEED)); +} + +void applyAutoSpeedThrottleDemand(int16_t *throttleCommand, timeUs_t currentTimeUs) +{ + if (!isFixedwingAutoSpeedActive()) return; + + static uint16_t autoSpeedThrottleCommand = PWM_RANGE_MIDDLE; + + if (posControl.flags.horizontalPositionDataNew && posControl.flags.verticalPositionDataNew) { + static timeUs_t lastUpdateTimeUs = 0; + timeUs_t dT = currentTimeUs - lastUpdateTimeUs; + lastUpdateTimeUs = currentTimeUs; + static pt1Filter_t speedToThrFilterState; + + if (dT > MAX_POSITION_UPDATE_INTERVAL_US) { + if (!speedToThrFilterState.RC) pt1FilterSetCutoff(&speedToThrFilterState, 1.0f / navConfig()->fw.auto_speed_thr_smoothing); + return; + } + + uint16_t minSpeed = 100 * navConfig()->fw.auto_speed_min_speed; + uint16_t maxSpeed = 100 * navConfig()->fw.auto_speed_max_speed; + uint16_t minThrottle = MAX(getThrottleIdleValue(), currentBatteryProfile->nav.fw.min_throttle); + uint16_t maxThrottle = currentBatteryProfile->nav.fw.max_throttle; + + posControl.desiredState.autoSpeedDemand = scaleRange(rxGetChannelValue(navConfig()->fw.auto_speed_channel - 1), PWM_RANGE_MIN, PWM_RANGE_MAX, minSpeed, maxSpeed); + uint16_t actualSpeed = posControl.actualState.vel3D; + posControl.autoSpeedSpdSource = FW_AUTO_SPD_GROUND; + uint16_t groundSpeedBoost = 0; + +#ifdef USE_PITOT + if (pitotValidateAirspeed()) { + static bool airspeedBoost = false; + // Pitot available and airspeed source selected or low airspeed boost applied when using ground speed source + if (!LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_AUTOSPEED_AIRSPEED) || airspeedBoost) { + actualSpeed = getAirspeedEstimate(); + if (airspeedBoost && actualSpeed > minSpeed) { + airspeedBoost = false; + } + posControl.autoSpeedSpdSource = FW_AUTO_SPD_AIR; + } else { // Ground speed source selected + // groundSpeedBoost used to increase ground speed demand when flying downwind using airspeed for correction + const uint16_t airSpeed = getAirspeedEstimate(); + const int16_t tailWind = actualSpeed - airSpeed; + if (tailWind > 0) { + groundSpeedBoost = MAX(tailWind - posControl.desiredState.autoSpeedDemand + minSpeed, 0); + } + + if (airSpeed < 0.95 * minSpeed) { + airspeedBoost = true; + } + } + if (airspeedBoost || groundSpeedBoost > 300) { + posControl.autoSpeedSpdSource = FW_AUTO_SPD_GROUND_OVERRIDE; + } + } else +#endif + { // Ground speed source with no pitot - set minimum throttle to auto_speed_min_throttle with pitch2throttle correction to prevent downwind stall + minThrottle = constrain(currentBatteryProfile->nav.fw.auto_speed_level_min_thr + fixedWingPitchToThrottleCorrection(-attitude.values.pitch, currentTimeUs), + minThrottle, maxThrottle); + } + + const uint16_t desiredSpeed = posControl.desiredState.autoSpeedDemand + groundSpeedBoost; + int16_t throttleCorr = navPidApply2(&posControl.pids.fw_autoSpeed, desiredSpeed, actualSpeed, US2S(dT), -PWM_RANGE_HALF, PWM_RANGE_HALF, 0); + throttleCorr = pt1FilterApply3(&speedToThrFilterState, throttleCorr, US2S(dT)); + + autoSpeedThrottleCommand = PWM_RANGE_MIDDLE + throttleCorr; + + // Boost throttle if ground speed too low + bool speedBoostReq = (getMinGroundSpeed(navConfig()->general.min_ground_speed) * 100.0f - posControl.actualState.velXY > 0) && !throttleSpeedAdjustment; + if (speedBoostReq || throttleSpeedAdjustment) { + autoSpeedThrottleCommand += applyFixedWingMinSpeedController(currentTimeUs); + } + + autoSpeedThrottleCommand = constrain(autoSpeedThrottleCommand, minThrottle, maxThrottle); + + // Indicate that information is no longer usable + posControl.flags.horizontalPositionDataConsumed = true; + } + + *throttleCommand = autoSpeedThrottleCommand; +} + /*----------------------------------------------------------- * Calculate loiter target based on current position and velocity *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index d3099eb2465..9b6646952c8 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -786,7 +786,7 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs) if (acc.accADCf[Z] < 1.0f && baroAltRate < -200.0f) { const uint16_t idleThrottle = getThrottleIdleValue(); const uint16_t hoverThrottleRange = currentBatteryProfile->nav.mc.hover_throttle - idleThrottle; - return rcCommand[THROTTLE] < idleThrottle + ((navigationInAutomaticThrottleMode() ? 0.8 : 0.5) * hoverThrottleRange); + return rcCommand[THROTTLE] < idleThrottle + ((navigationRequiresAutoThrottleMode() ? 0.8 : 0.5) * hoverThrottleRange); } } else if (acc.accADCf[Z] <= 1.0f) { gSpikeDetectTimeMs = 0; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index da4d27c6710..687d8802b39 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -113,7 +113,6 @@ typedef struct navigationFlags_s { bool rthTrackbackActive; // Activation status of RTH trackback bool wpTurnSmoothingActive; // Activation status WP turn smoothing bool manualEmergLandActive; // Activation status of manual emergency landing - #ifdef USE_GEOZONE bool sendToActive; bool forcedPosholdActive; @@ -145,6 +144,7 @@ typedef struct { fpVector3_t vel; int32_t yaw; int16_t climbRateDemand; + uint16_t autoSpeedDemand; } navigationDesiredState_t; typedef enum { @@ -352,6 +352,8 @@ typedef enum { NAV_MIXERAT = (1 << 16), // MIXERAT in progress NAV_CTL_HOLD = (1 << 17), // Nav loiter active at position + + NAV_CTL_SPEED = (1 << 18), // Auto speed allowed } navigationFSMStateFlags_t; typedef struct { @@ -422,6 +424,12 @@ typedef enum { RTH_HOME_FINAL_LAND, // Home position and altitude } rthTargetMode_e; +typedef enum { + FW_AUTO_SPD_GROUND, + FW_AUTO_SPD_AIR, + FW_AUTO_SPD_GROUND_OVERRIDE, +} fwAutoSpeedSpdSource_e; + #ifdef USE_GEOZONE typedef struct navSendTo_s { fpVector3_t targetPos; @@ -509,6 +517,8 @@ typedef struct { navSendTo_t sendTo; // Used for Geozones #endif + uint8_t autoSpeedSpdSource; // Auto Speed mode speed source + /* 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 8c988d78384..009b2d486f8 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -538,9 +538,17 @@ static int logicConditionCompute( } else { LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX); } - return true; + return true; break; #endif + case LOGIC_CONDITION_DISABLE_AUTOSPEED_AIRSPEED: + if (operandA > 0) { + LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_AUTOSPEED_AIRSPEED); + } else { + LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_AUTOSPEED_AIRSPEED); + } + return true; + break; default: return false; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 183c0cfa054..517074fe18b 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -90,6 +90,7 @@ typedef enum { LOGIC_CONDITION_ACOS = 58, LOGIC_CONDITION_ASIN = 59, LOGIC_CONDITION_ATAN2 = 60, + LOGIC_CONDITION_DISABLE_AUTOSPEED_AIRSPEED = 61, LOGIC_CONDITION_LAST } logicOperation_e; @@ -211,6 +212,7 @@ typedef enum { LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11), #endif LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED = (1 << 12), + LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_AUTOSPEED_AIRSPEED = (1 << 13), } logicConditionsGlobalFlags_t; typedef enum { diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 5b5bcea50c3..2c0125b1970 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -29,7 +29,8 @@ #define PWM_RANGE_MIN 1000 #define PWM_RANGE_MAX 2000 -#define PWM_RANGE_MIDDLE (PWM_RANGE_MIN + ((PWM_RANGE_MAX - PWM_RANGE_MIN) / 2)) +#define PWM_RANGE_HALF ((PWM_RANGE_MAX - PWM_RANGE_MIN) / 2) +#define PWM_RANGE_MIDDLE (PWM_RANGE_MIN + PWM_RANGE_HALF) #define PWM_PULSE_MIN 750 // minimum PWM pulse width which is considered valid #define PWM_PULSE_MAX 2250 // maximum PWM pulse width which is considered valid diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index da8a6237bd0..dc9d0ec270a 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -155,6 +155,7 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) .pitch_to_throttle = SETTING_NAV_FW_PITCH2THR_DEFAULT, // pwm units per degree of pitch (10pwm units ~ 1% throttle) .launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT, .launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT, // Motor idle or MOTOR_STOP + .auto_speed_level_min_thr = SETTING_FW_AUTO_SPEED_LEVEL_MIN_THR_DEFAULT, // 1300 us } }, diff --git a/src/main/sensors/battery_config_structs.h b/src/main/sensors/battery_config_structs.h index 8fe49295f8e..07b549a02eb 100644 --- a/src/main/sensors/battery_config_structs.h +++ b/src/main/sensors/battery_config_structs.h @@ -117,20 +117,19 @@ typedef struct batteryProfile_s { uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500. struct { - struct { - uint16_t hover_throttle; // multicopter hover throttle + uint16_t hover_throttle; // multicopter hover throttle } mc; struct { - uint16_t cruise_throttle; // Cruise throttle - uint16_t min_throttle; // Minimum allowed throttle in auto mode - uint16_t max_throttle; // Maximum allowed throttle in auto mode - uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10) - uint16_t launch_idle_throttle; // Throttle to keep at launch idle - uint16_t launch_throttle; // Launch throttle + uint16_t cruise_throttle; // Cruise throttle + uint16_t min_throttle; // Minimum allowed throttle in auto mode + uint16_t max_throttle; // Maximum allowed throttle in auto mode + uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10) + uint16_t launch_idle_throttle; // Throttle to keep at launch idle + uint16_t launch_throttle; // Launch throttle + uint16_t auto_speed_level_min_thr; // Minimum allowed auto speed throttle during level flight } fw; - } nav; #if defined(USE_POWER_LIMITS)