diff --git a/docs/Settings.md b/docs/Settings.md index af81a3c860c..2665c807778 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1346,6 +1346,26 @@ S.Port telemetry: If `ON`, send the legacy telemetry IDs for modes (Tmp1) and GN --- +### fw_auto_speed_max_speed + +Maximum ground speed for fixed wing auto speed mode [m/s]. + +| Default | Min | Max | +| --- | --- | --- | +| 22 | 5 | 50 | + +--- + +### fw_auto_speed_min_speed + +Minimum ground speed for fixed wing auto speed mode [m/s]. + +| Default | Min | Max | +| --- | --- | --- | +| 11 | 5 | 50 | + +--- + ### fw_autotune_max_rate_deflection The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`. @@ -3407,6 +3427,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 (Fixed wing). + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 255 | + +--- + +### nav_fw_auto_speed_i + +I gain of auto speed PID controller (Fixed wing). + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 255 | + +--- + +### nav_fw_auto_speed_p + +P gain of auto speed PID controller (Fixed wing). + +| 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 @@ -5936,6 +5986,7 @@ Selection of pitot hardware. VIRTUAL only works if a GPS is enabled. | FAKE | | | MSP | | | DLVR-L10D | | +| MS5525 | | --- 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 2dbdf9e474b..c909955009b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2262,6 +2262,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 (Fixed wing)." + 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 (Fixed wing)." + 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 (Fixed wing)." + 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 @@ -3000,6 +3018,18 @@ groups: field: fw.cruise_speed min: 0 max: 65535 + - name: fw_auto_speed_min_speed + description: "Minimum ground speed for fixed wing 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 ground speed for fixed wing auto speed mode [m/s]." + default_value: 22 + field: fw.auto_speed_max_speed + min: 5 + max: 50 - 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..40fadf9b872 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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 003ae97dded..0c83a8d0dfd 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -257,6 +257,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 } } @@ -550,7 +556,7 @@ void updatePIDCoefficients(void) for (int axis = 0; axis < 3; axis++) { pidState[axis].stickPosition = constrain(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE, -500, 500) / 500.0f; } - + float tpaFactor=1.0f; float iTermFactor=1.0f; // Separate factor for I-term scaling if(usedPidControllerType == PID_TYPE_PIFF){ // Fixed wing TPA calculation @@ -570,13 +576,13 @@ void updatePIDCoefficients(void) } tpaFactorprev = tpaFactor; - + // If nothing changed - don't waste time recalculating coefficients if (!pidGainsUpdateRequired) { return; } - + // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change for (int axis = 0; axis < 3; axis++) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index ff2e85031b7..3498571e9fb 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 8ea42ade97e..88ef3fc0fde 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1161,7 +1161,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; @@ -1988,6 +1988,20 @@ 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)) { + strcpy(buff, "(ASPD)"); + if (isFixedwingAutoSpeedActive()) { + osdFormatVelocityStr(buff + 1, getDesiredAutoSpeed(), OSD_SPEED_TYPE_3D, false); + buff[5] = ')'; + buff[6] = '\0'; + } + break; + } else { + displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); + return true; + } + case OSD_GLIDESLOPE: { float horizontalSpeed = gpsSol.groundSpeed; @@ -2216,7 +2230,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_GPS_EXTRA_STATS: { gpsSetOsdMonRfWidgetEnabled(true); - + // Collect satellite stats grouped by GNSS ID: 0,2,3,6 uint8_t stats[4][2] = { {0,0}, {0,0}, {0,0}, {0,0} }; // [group][0]=total, [group][1]=good for (int i = 0; i < UBLOX_MAX_SIGNALS; ++i) { @@ -4360,6 +4374,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); @@ -4418,7 +4433,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_MISSION] = OSD_POS(0, 10); osdLayoutsConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10); + osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10); osdLayoutsConfig->item_pos[0][OSD_GPS_EXTRA_STATS] = OSD_POS(3, 11); osdLayoutsConfig->item_pos[0][OSD_GPS_LAT] = OSD_POS(0, 12); 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 d640e2b25c3..13735950406 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,8 @@ 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 .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 +408,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 +444,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 +480,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 +502,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 +541,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 +563,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 +605,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 +625,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 +645,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 +667,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 +688,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 +776,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 +795,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 +817,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 +842,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 +888,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 +902,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 +1071,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 +1092,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 +1113,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, @@ -3560,7 +3562,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(); @@ -3874,10 +3876,10 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData) int isGCSValid(void) { - return (ARMING_FLAG(ARMED) && - (posControl.flags.estPosStatus >= EST_TRUSTED) && - posControl.gpsOrigin.valid && - posControl.flags.isGCSAssistedNavigationEnabled && + return (ARMING_FLAG(ARMED) && + (posControl.flags.estPosStatus >= EST_TRUSTED) && + posControl.gpsOrigin.valid && + posControl.flags.isGCSAssistedNavigationEnabled && (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)); } @@ -4409,6 +4411,7 @@ void applyWaypointNavigationAndAltitudeHold(void) applyRoverBoatNavigationController(navStateFlags, currentTimeUs); } else if (STATE(FIXED_WING_LEGACY)) { applyFixedWingNavigationController(navStateFlags, currentTimeUs); + getAutoSpeedThrottleDemand(&rcCommand[THROTTLE]); } else { applyMulticopterNavigationController(navStateFlags, currentTimeUs); @@ -5056,6 +5059,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) @@ -5251,7 +5262,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)) || @@ -5261,7 +5272,7 @@ bool navigationInAutomaticThrottleMode(void) bool navigationIsControllingThrottle(void) { // 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() || isFixedwingAutoSpeedActive()) && 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..8d7eb3f588d 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,8 @@ 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 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 +600,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 +779,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); @@ -820,6 +823,10 @@ int8_t navCheckActiveAngleHoldAxis(void); uint8_t getActiveWpNumber(void); uint16_t getFlownLoiterRadius(void); +uint16_t getDesiredAutoSpeed(void); +bool isFixedwingAutoSpeedActive(void); +void getAutoSpeedThrottleDemand(int16_t *throttleCommand); + /* Returns the heading recorded when home position was acquired. * Note that the navigation system uses deg*100 as unit and angles * are in the [0, 360 * 100) interval. diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 2051f64232f..5ab7c18d47d 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -598,35 +598,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 (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 have new position - update min speed controller + if (posControl.flags.horizontalPositionDataNew) { + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + previousTimePositionUpdate = currentTimeUs; - // 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; @@ -680,36 +669,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); + } - uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle); + // 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); + } + } - // 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); + uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_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] - currentBatteryProfile->nav.fw.cruise_throttle); + } else { + correctedThrottleValue = getMaxThrottle(); + } + isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle); } 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 @@ -861,6 +858,70 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs) } } +uint16_t getDesiredAutoSpeed(void) +{ + return posControl.desiredState.autoSpeedDemand; +} + +bool isFixedwingAutoSpeedActive(void) +{ + return STATE(AIRPLANE) && ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXAUTOSPEED) && !FLIGHT_MODE(FAILSAFE_MODE) && + posControl.flags.estVelStatus == EST_TRUSTED && posControl.flags.estAltStatus == EST_TRUSTED && + !(navigationRequiresAutoThrottleMode() && !(navGetCurrentStateFlags() & NAV_CTL_SPEED)); +} + +void getAutoSpeedThrottleDemand(int16_t *throttleCommand) +{ + if (!isFixedwingAutoSpeedActive()) return; + + static uint16_t autoSpeedThrottleCommand = PWM_RANGE_MIDDLE; + + if (posControl.flags.horizontalPositionDataNew && posControl.flags.verticalPositionDataNew) { + static timeUs_t lastUpdateTimeUs = 0; + timeUs_t currentTime = micros(); + timeUs_t dT = currentTime - lastUpdateTimeUs; + lastUpdateTimeUs = currentTime; + + if (dT > MAX_POSITION_UPDATE_INTERVAL_US) return; // skip if update is delayed + + static pt1Filter_t speedToThrFilterState; + + uint16_t minSpeed = 100 * navConfig()->fw.auto_speed_min_speed; + uint16_t maxSpeed = 100 * navConfig()->fw.auto_speed_max_speed; + uint16_t minThrottle = getThrottleIdleValue(); + + posControl.desiredState.autoSpeedDemand = scaleRange(rxGetChannelValue(THROTTLE), PWM_RANGE_MIN, PWM_RANGE_MAX, minSpeed, maxSpeed); + uint16_t actualSpeed = calc_length_pythagorean_2D(posControl.actualState.velXY, posControl.actualState.abs.vel.z); + +#ifdef USE_PITOT + if (pitotValidForAirspeed()) { // Use airspeed if pitot available + actualSpeed = getAirspeedEstimate(); + } else +#endif + { // When using ground speed set minimum throttle to cruise throttle with pitch2throttle correction to prevent downwind stall + minThrottle = currentBatteryProfile->nav.fw.cruise_throttle + fixedWingPitchToThrottleCorrection(-attitude.values.pitch, currentTime); + } + + int16_t throttleCorr = navPidApply2(&posControl.pids.fw_autoSpeed, posControl.desiredState.autoSpeedDemand, actualSpeed, US2S(dT), -PWM_RANGE_HALF, PWM_RANGE_HALF, 0); + throttleCorr = pt1FilterApply4(&speedToThrFilterState, throttleCorr, 0.5f, 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(currentTime); + } + + autoSpeedThrottleCommand = constrain(autoSpeedThrottleCommand, minThrottle, getMaxThrottle()); + + // 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 5d4b7a5b425..91b9b3dd013 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -785,7 +785,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 a1f07e470c0..611d90ba391 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -144,6 +144,7 @@ typedef struct { fpVector3_t vel; int32_t yaw; int16_t climbRateDemand; + uint16_t autoSpeedDemand; } navigationDesiredState_t; typedef enum { @@ -351,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 { 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/pitotmeter.c b/src/main/sensors/pitotmeter.c index 3618923614a..649c42954c8 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -456,7 +456,7 @@ bool pitotValidForAirspeed(void) // For virtual pitot, we need GPS fix if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { - ret = ret && STATE(GPS_FIX); + ret = ret && STATE(GPS_FIX) && isEstimatedWindSpeedValid(); } // For hardware pitot sensors, validate readings against GPS when armed