Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

Revert "remove attimode, add acro stabilise"

Browse source 
This reverts commit 6f2c501431.
This commit is contained in:
breadoven 2023-09-22 21:30:27 +01:00
parent 79585dc611
commit 32fcb13823
16 changed files with 61 additions and 52 deletions
Download patch file Download diff file Expand all files Collapse all files

10
docs/Settings.md
View file

@ -1072,16 +1072,6 @@ S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer da
---
### fw_acro_atti_stab
Activates attitude stabilisation for ACRO mode when stick centered and roll/pitch angles within ANGLE mode bank angle limits (non inverted flight only)
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### fw_autotune_max_rate_deflection
The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`.

5
src/main/fc/fc_core.c
View file

@ -644,15 +644,18 @@ void processRx(timeUs_t currentTimeUs)
processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData);
}
/* Disable modes initially, will be enabled as required with priority ANGLE > HORIZON */
/* Disable modes initially, will be enabled as required with priority ANGLE > HORIZON > ATTITUDE HOLD */
DISABLE_FLIGHT_MODE(ANGLE_MODE);
DISABLE_FLIGHT_MODE(HORIZON_MODE);
DISABLE_FLIGHT_MODE(ATTIHOLD_MODE);
if (sensors(SENSOR_ACC)) {
if (IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) {
ENABLE_FLIGHT_MODE(ANGLE_MODE);
} else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) {
ENABLE_FLIGHT_MODE(HORIZON_MODE);
} else if (STATE(AIRPLANE) && IS_RC_MODE_ACTIVE(BOXATTIHOLD)) {
ENABLE_FLIGHT_MODE(ATTIHOLD_MODE);
}
}

5
src/main/fc/fc_msp_box.c
View file

@ -99,6 +99,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ .boxId = BOXCHANGEMISSION, .boxName = "MISSION CHANGE", .permanentId = 59 },
{ .boxId = BOXBEEPERMUTE, .boxName = "BEEPER MUTE", .permanentId = 60 },
{ .boxId = BOXMULTIFUNCTION, .boxName = "MULTI FUNCTION", .permanentId = 61 },
{ .boxId = BOXATTIHOLD, .boxName = "ATTITUDE HOLD", .permanentId = 62 },
{ .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF }
};
@ -275,6 +276,9 @@ void initActiveBoxIds(void)
if (sensors(SENSOR_BARO)) {
ADD_ACTIVE_BOX(BOXAUTOLEVEL);
}
if (sensors(SENSOR_ACC)) {
ADD_ACTIVE_BOX(BOXATTIHOLD);
}
}
/*
@ -419,6 +423,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
#ifdef USE_MULTI_FUNCTIONS
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)), BOXMULTIFUNCTION);
#endif
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXATTIHOLD)), BOXATTIHOLD);
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
for (uint32_t i = 0; i < activeBoxIdCount; i++) {
if (activeBoxes[activeBoxIds[i]]) {

1
src/main/fc/rc_modes.h
View file

@ -79,6 +79,7 @@ typedef enum {
BOXCHANGEMISSION = 50,
BOXBEEPERMUTE = 51,
BOXMULTIFUNCTION = 52,
BOXATTIHOLD = 53,
CHECKBOX_ITEM_COUNT
} boxId_e;

3
src/main/fc/runtime_config.c
View file

@ -176,6 +176,9 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
if (FLIGHT_MODE(HORIZON_MODE))
return FLM_HORIZON;
if (FLIGHT_MODE(ATTIHOLD_MODE))
return FLM_ATTIHOLD;
return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO;
}

2
src/main/fc/runtime_config.h
View file

@ -104,6 +104,7 @@ typedef enum {
TURN_ASSISTANT = (1 << 14),
TURTLE_MODE = (1 << 15),
SOARING_MODE = (1 << 16),
ATTIHOLD_MODE = (1 << 17),
} flightModeFlags_e;
extern uint32_t flightModeFlags;
@ -161,6 +162,7 @@ typedef enum {
FLM_CRUISE,
FLM_LAUNCH,
FLM_FAILSAFE,
FLM_ATTIHOLD,
FLM_COUNT
} flightModeForTelemetry_e;

5
src/main/fc/settings.yaml
View file

@ -2141,11 +2141,6 @@ groups:
field: fixedWingLevelTrimGain
min: 0
max: 20
- name: fw_acro_atti_stab
description: "Activates attitude stabilisation for ACRO mode when stick centered and roll/pitch angles within ANGLE mode bank angle limits (non inverted flight only)"
default_value: OFF
field: fixedWingAcroAttiStab
type: bool
- name: PG_PID_AUTOTUNE_CONFIG
type: pidAutotuneConfig_t

51
src/main/flight/pid.c
View file

@ -167,7 +167,7 @@ static EXTENDED_FASTRAM bool levelingEnabled = false;
static EXTENDED_FASTRAM float fixedWingLevelTrim;
static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 7);
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 6);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = {
@ -298,8 +298,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT,
.fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT,
.fixedWingAcroAttiStab = SETTING_FW_ACRO_ATTI_STAB_DEFAULT,
#ifdef USE_SMITH_PREDICTOR
.smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT,
.smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT,
@ -1109,39 +1107,42 @@ void FAST_CODE pidController(float dT)
#endif
}
// Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ACRO attitude stabilisation
// Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ATTI_MODE
const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f;
levelingEnabled = false;
static bool resetAcroStab = true;
static bool restartAttiMode = true;
if (!restartAttiMode) {
restartAttiMode = !FLIGHT_MODE(ATTIHOLD_MODE); // set restart flag if attihold_mode not active
}
for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || isFlightAxisAngleOverrideActive(axis)) {
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ATTIHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) {
// If axis angle override, get the correct angle from Logic Conditions
float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis));
if (FLIGHT_MODE(ATTIHOLD_MODE) && !isFlightAxisAngleOverrideActive(axis)) {
static int16_t attiHoldTarget[2];
if (restartAttiMode) { // set target attitude to current attitude when initialised
attiHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
attiHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH];
restartAttiMode = false;
}
uint16_t bankLimit = pidProfile()->max_angle_inclination[axis];
if (calculateRollPitchCenterStatus() == CENTERED) {
angleTarget = constrain(attiHoldTarget[axis], -bankLimit, bankLimit);
} else {
angleTarget = constrain(attitude.raw[axis] + angleTarget, -bankLimit, bankLimit);
attiHoldTarget[axis] = attitude.raw[axis];
}
}
// Apply the Level PID controller
pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON
levelingEnabled = true;
resetAcroStab = true;
} else if (STATE(AIRPLANE) && pidProfile()->fixedWingAcroAttiStab) {
/* Provides fixed wing acro attitude stabilisation to avoid attitude drift at zero rates
* Only active for non inverted flight within Angle mode bank angle limits */
static int16_t attiHoldTarget[2];
if (resetAcroStab) { // set target attitude to current attitude when initialised
attiHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
attiHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH];
resetAcroStab = false;
}
uint16_t bankLimit = pidProfile()->max_angle_inclination[axis];
bool isNotinverted = ABS(attitude.raw[FD_ROLL]) < pidProfile()->max_angle_inclination[FD_ROLL];
if (calculateRollPitchCenterStatus() == CENTERED && ABS(attitude.raw[axis]) <= bankLimit && isNotinverted) {
pidLevel(constrain(attiHoldTarget[axis], -bankLimit, bankLimit), &pidState[axis], axis, horizonRateMagnitude, dT);
} else {
attiHoldTarget[axis] = attitude.raw[axis];
}
}
}

3
src/main/flight/pid.h
View file

@ -155,9 +155,6 @@ typedef struct pidProfile_s {
float fixedWingLevelTrim;
float fixedWingLevelTrimGain;
bool fixedWingAcroAttiStab;
#ifdef USE_SMITH_PREDICTOR
float smithPredictorStrength;
float smithPredictorDelay;

2
src/main/io/osd.c
View file

@ -2212,6 +2212,8 @@ static bool osdDrawSingleElement(uint8_t item)
p = "ANGL";
else if (FLIGHT_MODE(HORIZON_MODE))
p = "HOR ";
else if (FLIGHT_MODE(ATTIHOLD_MODE))
p = "ATTI";
displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
return true;

1
src/main/io/osd_dji_hd.c
View file

@ -300,6 +300,7 @@ static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
case FLM_ALTITUDE_HOLD:
case FLM_POSITION_HOLD:
case FLM_MISSION:
case FLM_ATTIHOLD:
default:
// Unsupported ATM, keep at ANGLE
bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE

4
src/main/programming/logic_condition.c
View file

@ -835,6 +835,10 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
return (bool) FLIGHT_MODE(HORIZON_MODE);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ATTIMODE:
return (bool) FLIGHT_MODE(ATTIHOLD_MODE);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR:
return (bool) FLIGHT_MODE(AIRMODE_ACTIVE);
break;

1
src/main/programming/logic_condition.h
View file

@ -154,6 +154,7 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4, // 13
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO, // 14
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION, // 15
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ATTIMODE, // 16
} logicFlightModeOperands_e;
typedef enum {

2
src/main/telemetry/crsf.c
View file

@ -352,6 +352,8 @@ static void crsfFrameFlightMode(sbuf_t *dst)
flightMode = "ANGL";
} else if (FLIGHT_MODE(HORIZON_MODE)) {
flightMode = "HOR";
} else if (FLIGHT_MODE(ATTIHOLD_MODE)) {
flightMode = "ATTI";
}
#ifdef USE_GPS
} else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {

2
src/main/telemetry/mavlink.c
View file

@ -191,6 +191,7 @@ static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode)
case FLM_ACRO_AIR: return COPTER_MODE_ACRO;
case FLM_ANGLE: return COPTER_MODE_STABILIZE;
case FLM_HORIZON: return COPTER_MODE_STABILIZE;
case FLM_ATTIHOLD: return COPTER_MODE_STABILIZE;
case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD;
case FLM_POSITION_HOLD: return COPTER_MODE_POSHOLD;
case FLM_RTH: return COPTER_MODE_RTL;
@ -220,6 +221,7 @@ static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode)
case FLM_ACRO_AIR: return PLANE_MODE_ACRO;
case FLM_ANGLE: return PLANE_MODE_FLY_BY_WIRE_A;
case FLM_HORIZON: return PLANE_MODE_STABILIZE;
case FLM_ATTIHOLD: return PLANE_MODE_STABILIZE;
case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B;
case FLM_POSITION_HOLD: return PLANE_MODE_LOITER;
case FLM_RTH: return PLANE_MODE_RTL;

2
src/main/telemetry/sim.c
View file

@ -122,7 +122,7 @@ static uint8_t simModuleState = SIM_MODULE_NOT_DETECTED;
static int simRssi;
static uint8_t accEvent = ACC_EVENT_NONE;
static char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " };
static char* modeDescriptions[] = { "MAN", "ACR", "AIR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "CRS", "LAU", "FS" };
static char* modeDescriptions[] = { "MAN", "ACR", "AIR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "CRS", "LAU", "FS", "ATT" };
static const char gpsFixIndicators[] = { '!', '*', ' ' };
static bool checkGroundStationNumber(uint8_t* rv)