diff --git a/docs/Cli.md b/docs/Cli.md index 1b28467429..58e75d9fc3 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -339,6 +339,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | fw_iterm_throw_limit | 165 | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. | | fw_reference_airspeed | 1000 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. | | fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | +| fw_loiter_direction | RIGHT | Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. | | mode_range_logic_operator | OR | Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. | | default_rate_profile | 0 | Default = profile number | | mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you acquire valid GPS fix. | diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index b7e6e74a4b..aec28f3726 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -84,6 +84,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXBRAKING, "MC BRAKING", 46 }, { BOXUSER1, "USER1", 47 }, { BOXUSER2, "USER2", 48 }, + { BOXLOITERDIRCHN, "LOITER CHANGE", 49 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -195,6 +196,9 @@ void initActiveBoxIds(void) const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; if (navFlowDeadReckoning || navReadyQuads || navReadyPlanes) { activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; + if (STATE(FIXED_WING)) { + activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN; + } } if (navReadyQuads || navReadyPlanes) { @@ -343,6 +347,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBRAKING)), BOXBRAKING); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER1)), BOXUSER1); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER2)), BOXUSER2); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)),BOXLOITERDIRCHN); 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 050e00bedb..1a90da2457 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -66,6 +66,7 @@ typedef enum { BOXUSER1 = 37, BOXUSER2 = 38, BOXFPVANGLEMIX = 39, + BOXLOITERDIRCHN = 40, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d9de5f2c59..e7679be9eb 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -49,6 +49,8 @@ tables: enum: gpsDynModel_e - name: reset_type values: ["NEVER", "FIRST_ARM", "EACH_ARM"] + - name: direction + values: ["RIGHT", "LEFT", "YAW"] - name: nav_user_control_mode values: ["ATTI", "CRUISE"] - name: nav_rth_alt_mode @@ -951,6 +953,10 @@ groups: field: fixedWingItermThrowLimit min: FW_ITERM_THROW_LIMIT_MIN max: FW_ITERM_THROW_LIMIT_MAX + - name: fw_loiter_direction + field: loiter_direction + condition: USE_NAV + table: direction - name: fw_reference_airspeed field: fixedWingReferenceAirspeed min: 1 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8b1477ed0a..5516343d4c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -194,6 +194,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingReferenceAirspeed = 1000, .fixedWingCoordinatedYawGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, + + .loiter_direction = NAV_LOITER_RIGHT, ); void pidInit(void) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index a069f6bc85..4a5c6320de 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -105,6 +105,8 @@ typedef struct pidProfile_s { float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point + + uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) } pidProfile_t; typedef struct pidAutotuneConfig_s { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index f48474fffd..6a7c9d19e9 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -54,6 +54,12 @@ enum { NAV_GPS_CRUISE = 1 // Pitch/roll stick controls velocity (forward/right speed) }; +enum { + NAV_LOITER_RIGHT = 0, // Loitering direction right + NAV_LOITER_LEFT = 1, // Loitering direction left + NAV_LOITER_YAW = 2 +}; + enum { NAV_RTH_NO_ALT = 0, // Maintain current altitude NAV_RTH_EXTRA_ALT = 1, // Maintain current altitude + predefined safety margin diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 0593104310..32dffbd4d8 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -43,6 +43,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "navigation/navigation.h" @@ -63,6 +64,7 @@ static bool isRollAdjustmentValid = false; static float throttleSpeedAdjustment = 0; static bool isAutoThrottleManuallyIncreased = false; static int32_t navHeadingError; +static int8_t loiterDirYaw = 1; /*----------------------------------------------------------- @@ -213,6 +215,18 @@ void resetFixedWingPositionController(void) pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f); } +static int8_t loiterDirection(void) { + int8_t dir = 1; //NAV_LOITER_RIGHT + if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) dir = -1; + if (pidProfile()->loiter_direction == NAV_LOITER_YAW) { + if (rcCommand[YAW] < -250) loiterDirYaw = 1; //RIGHT //yaw is contrariwise + if (rcCommand[YAW] > 250) loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c + dir = loiterDirYaw; + } + if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) dir *= -1; + return dir; +} + static void calculateVirtualPositionTarget_FW(float trackingPeriod) { float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x; @@ -233,7 +247,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // Calculate virtual position for straight movement if (needToCalculateCircularLoiter) { // We are closing in on a waypoint, calculate circular loiter - float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(45.0f); + float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f); float loiterTargetX = posControl.desiredState.pos.x + navConfig()->fw.loiter_radius * cos_approx(loiterAngle); float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle); @@ -292,7 +306,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta // If forced turn direction flag is enabled we fix the sign of the direction if (forceTurnDirection) { - navHeadingError = ABS(navHeadingError); + navHeadingError = loiterDirection() * ABS(navHeadingError); } // Slow error monitoring (2Hz rate) @@ -308,7 +322,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta // Only allow PID integrator to shrink if error is decreasing over time const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0); - // Input error in (deg*100), output pitch angle (deg*100) + // Input error in (deg*100), output roll angle (deg*100) float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.yaw + navHeadingError, posControl.actualState.yaw, US2S(deltaMicros), -DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),