Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 19:40:27 +03:00
Code Issues Wiki Activity

change mode name + fixes

Browse source
This commit is contained in:
breadoven 2023-10-27 15:39:20 +01:00
parent 98f7f43c1f
commit b461d8095b
16 changed files with 75 additions and 71 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -679,18 +679,19 @@ void processRx(timeUs_t currentTimeUs)
bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs);
bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce;
/* Disable modes initially, will be enabled as required with priority ANGLE > HORIZON > ATTITUDE HOLD */
/* Disable stabilised modes initially, will be enabled as required with priority ANGLE > HORIZON > ANGLEHOLD
* MANUAL mode has priority over these modes except when ANGLE auto enabled */
DISABLE_FLIGHT_MODE(ANGLE_MODE);
DISABLE_FLIGHT_MODE(HORIZON_MODE);
DISABLE_FLIGHT_MODE(ATTIHOLD_MODE);
DISABLE_FLIGHT_MODE(ANGLEHOLD_MODE);
if (sensors(SENSOR_ACC)) {
if (sensors(SENSOR_ACC) && (!FLIGHT_MODE(MANUAL_MODE) || autoEnableAngle)) {
if (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle) {
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);
} else if (STATE(AIRPLANE) && IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
ENABLE_FLIGHT_MODE(ANGLEHOLD_MODE);
}
}

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

@ -102,7 +102,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ .boxId = BOXMULTIFUNCTION, .boxName = "MULTI FUNCTION", .permanentId = 61 },
{ .boxId = BOXMIXERPROFILE, .boxName = "MIXER PROFILE 2", .permanentId = 62 },
{ .boxId = BOXMIXERTRANSITION, .boxName = "MIXER TRANSITION", .permanentId = 63 },
{ .boxId = BOXATTIHOLD, .boxName = "ATTITUDE HOLD", .permanentId = 64 },
{ .boxId = BOXANGLEHOLD, .boxName = "ANGLE HOLD", .permanentId = 64 },
{ .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF }
};
@ -281,7 +281,7 @@ void initActiveBoxIds(void)
ADD_ACTIVE_BOX(BOXAUTOLEVEL);
}
if (sensors(SENSOR_ACC)) {
ADD_ACTIVE_BOX(BOXATTIHOLD);
ADD_ACTIVE_BOX(BOXANGLEHOLD);
}
}
@ -436,7 +436,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(currentMixerProfileIndex), BOXMIXERPROFILE);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION);
#endif
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXATTIHOLD)), BOXATTIHOLD);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)), BOXANGLEHOLD);
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
for (uint32_t i = 0; i < activeBoxIdCount; i++) {
if (activeBoxes[activeBoxIds[i]]) {

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

@ -81,7 +81,7 @@ typedef enum {
BOXMULTIFUNCTION = 52,
BOXMIXERPROFILE = 53,
BOXMIXERTRANSITION = 54,
BOXATTIHOLD = 55,
BOXANGLEHOLD = 55,
CHECKBOX_ITEM_COUNT
} boxId_e;

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

@ -158,8 +158,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
if (FLIGHT_MODE(HORIZON_MODE))
return FLM_HORIZON;
if (FLIGHT_MODE(ATTIHOLD_MODE))
return FLM_ATTIHOLD;
if (FLIGHT_MODE(ANGLEHOLD_MODE))
return FLM_ANGLEHOLD;
return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO;
}

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

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

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

@ -159,7 +159,8 @@ typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t
static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
static EXTENDED_FASTRAM bool levelingEnabled = false;
static EXTENDED_FASTRAM bool attiHoldIsLevel = false;
static EXTENDED_FASTRAM bool restartAngleHoldMode = true;
static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
#define FIXED_WING_LEVEL_TRIM_MAX_ANGLE 10.0f // Max angle auto trimming can demand
#define FIXED_WING_LEVEL_TRIM_DIVIDER 50.0f
@ -1065,58 +1066,57 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
}
bool isAttiholdLevel(void)
bool isAngleHoldLevel(void)
{
return attiHoldIsLevel;
return angleHoldIsLevel;
}
void updateAttihold(float *angleTarget, uint8_t axis)
void updateAngleHold(float *angleTarget, uint8_t axis)
{
int8_t navAttiHoldAxis = navCheckActiveAttiHoldAxis();
static bool restartAttiMode = true;
int8_t navAngleHoldAxis = navCheckActiveAngleHoldAxis();
// static bool restartAngleHoldMode = true;
if (!restartAttiMode) { // reset flags for when attitude hold is inactive
restartAttiMode = !FLIGHT_MODE(ATTIHOLD_MODE) && navAttiHoldAxis == -1;
attiHoldIsLevel = restartAttiMode ? false: attiHoldIsLevel;
if (!restartAngleHoldMode) { // set restart flag when attitude hold is inactive
restartAngleHoldMode = !FLIGHT_MODE(ANGLEHOLD_MODE) && navAngleHoldAxis == -1;
}
if ((FLIGHT_MODE(ATTIHOLD_MODE) || axis == navAttiHoldAxis) && !isFlightAxisAngleOverrideActive(axis)) {
/* attiHoldTarget stores attitude values using a zero datum when level.
* This requires attiHoldTarget pitch to be corrected for fixedWingLevelTrim so it is 0
if ((FLIGHT_MODE(ANGLEHOLD_MODE) || axis == navAngleHoldAxis) && !isFlightAxisAngleOverrideActive(axis)) {
/* angleHoldTarget stores attitude values using a zero datum when level.
* This requires angleHoldTarget pitch to be corrected for fixedWingLevelTrim so it is 0
* when the craft is level even though attitude pitch is non zero in this case.
* angleTarget pitch is corrected back to fixedWingLevelTrim datum on return from function */
static int16_t attiHoldTarget[2];
static int16_t angleHoldTarget[2];
if (restartAttiMode) { // set target attitude to current attitude on activation
attiHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
attiHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH] + DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);
restartAttiMode = false;
if (restartAngleHoldMode) { // set target attitude to current attitude on activation
angleHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
angleHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH] + DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);
restartAngleHoldMode = false;
}
// set flag indicating attitude hold is level
if (FLIGHT_MODE(ATTIHOLD_MODE)) {
attiHoldIsLevel = attiHoldTarget[FD_ROLL] == 0 && attiHoldTarget[FD_PITCH] == 0;
if (FLIGHT_MODE(ANGLEHOLD_MODE)) {
angleHoldIsLevel = angleHoldTarget[FD_ROLL] == 0 && angleHoldTarget[FD_PITCH] == 0;
} else {
attiHoldIsLevel = attiHoldTarget[navAttiHoldAxis] == 0;
angleHoldIsLevel = angleHoldTarget[navAngleHoldAxis] == 0;
}
uint16_t bankLimit = pidProfile()->max_angle_inclination[axis];
// use Nav bank angle limits if Nav active
if (navAttiHoldAxis == FD_ROLL) {
if (navAngleHoldAxis == FD_ROLL) {
bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle);
} else if (navAttiHoldAxis == FD_PITCH) {
} else if (navAngleHoldAxis == FD_PITCH) {
bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
}
int16_t levelTrim = axis == FD_PITCH ? DEGREES_TO_DECIDEGREES(fixedWingLevelTrim) : 0;
if (calculateRollPitchCenterStatus() == CENTERED) {
attiHoldTarget[axis] = ABS(attiHoldTarget[axis]) < 30 ? 0 : attiHoldTarget[axis]; // snap to level when within 3 degs of level
*angleTarget = constrain(attiHoldTarget[axis] - levelTrim, -bankLimit, bankLimit);
angleHoldTarget[axis] = ABS(angleHoldTarget[axis]) < 30 ? 0 : angleHoldTarget[axis]; // snap to level when within 3 degs of level
*angleTarget = constrain(angleHoldTarget[axis] - levelTrim, -bankLimit, bankLimit);
} else {
*angleTarget = constrain(attitude.raw[axis] + *angleTarget + levelTrim, -bankLimit, bankLimit);
attiHoldTarget[axis] = attitude.raw[axis] + levelTrim;
angleHoldTarget[axis] = attitude.raw[axis] + levelTrim;
}
}
}
@ -1174,20 +1174,23 @@ void FAST_CODE pidController(float dT)
// Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ATTI_MODE
const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f;
levelingEnabled = false;
angleHoldIsLevel = false;
for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ATTIHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) {
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ANGLEHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) {
// If axis angle override, get the correct angle from Logic Conditions
float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis));
if (STATE(AIRPLANE)) { // update attitude hold mode
updateAttihold(&angleTarget, axis);
updateAngleHold(&angleTarget, axis);
}
// Apply the Level PID controller
pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON
levelingEnabled = true;
} else {
restartAngleHoldMode = true;
}
}
@ -1335,7 +1338,7 @@ bool isFixedWingLevelTrimActive(void)
return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !areSticksDeflected() &&
(FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) &&
!FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) &&
!navigationIsControllingAltitude() && !(navCheckActiveAttiHoldAxis() == FD_PITCH && !attiHoldIsLevel);
!navigationIsControllingAltitude() && !(navCheckActiveAngleHoldAxis() == FD_PITCH && !angleHoldIsLevel);
}
void updateFixedWingLevelTrim(timeUs_t currentTimeUs)

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

@ -220,4 +220,4 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex);
bool isFixedWingLevelTrimActive(void);
void updateFixedWingLevelTrim(timeUs_t currentTimeUs);
float getFixedWingLevelTrim(void);
bool isAttiholdLevel(void);
bool isAngleHoldLevel(void);

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

@ -2266,8 +2266,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";
else if (FLIGHT_MODE(ANGLEHOLD_MODE))
p = "AHLD";
displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
return true;
@ -5198,14 +5198,14 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
if (STATE(LANDING_DETECTED)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
}
if (IS_RC_MODE_ACTIVE(BOXATTIHOLD)) {
int8_t navAttiHoldAxis = navCheckActiveAttiHoldAxis();
if (isAttiholdLevel()) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ATTI_LEVEL);
} else if (navAttiHoldAxis == FD_ROLL) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ATTI_ROLL);
} else if (navAttiHoldAxis == FD_PITCH) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ATTI_PITCH);
if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
int8_t navAngleHoldAxis = navCheckActiveAngleHoldAxis();
if (isAngleHoldLevel()) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_LEVEL);
} else if (navAngleHoldAxis == FD_ROLL) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_ROLL);
} else if (navAngleHoldAxis == FD_PITCH) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH);
}
}
}

6
src/main/io/osd.h
View file

@ -118,9 +118,9 @@
#define OSD_MSG_UNABLE_ARM "UNABLE TO ARM"
#define OSD_MSG_SAVING_SETTNGS "** SAVING SETTINGS **"
#define OSD_MSG_SETTINGS_SAVED "** SETTINGS SAVED **"
#define OSD_MSG_ATTI_ROLL "(ATTI ROLL)"
#define OSD_MSG_ATTI_PITCH "(ATTI PITCH)"
#define OSD_MSG_ATTI_LEVEL "(ATTI LEVEL)"
#define OSD_MSG_ANGLEHOLD_ROLL "(ANGLEHOLD ROLL)"
#define OSD_MSG_ANGLEHOLD_PITCH "(ANGLEHOLD PITCH)"
#define OSD_MSG_ANGLEHOLD_LEVEL "(ANGLEHOLD LEVEL)"
#ifdef USE_DEV_TOOLS
#define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED"

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

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

4
src/main/navigation/navigation.c
View file

@ -4566,11 +4566,11 @@ int32_t navigationGetHeadingError(void)
return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog);
}
int8_t navCheckActiveAttiHoldAxis(void)
int8_t navCheckActiveAngleHoldAxis(void)
{
int8_t activeAxis = -1;
if (IS_RC_MODE_ACTIVE(BOXATTIHOLD)) {
if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
bool altholdActive = stateFlags & NAV_REQUIRE_ANGLE_FW && !(stateFlags & NAV_REQUIRE_ANGLE);

2
src/main/navigation/navigation.h
View file

@ -627,7 +627,7 @@ bool isEstimatedAglTrusted(void);
void checkManualEmergencyLandingControl(bool forcedActivation);
float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue);
int8_t navCheckActiveAttiHoldAxis(void);
int8_t navCheckActiveAngleHoldAxis(void);
/* Returns the heading recorded when home position was acquired.
* Note that the navigation system uses deg*100 as unit and angles

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

@ -849,8 +849,8 @@ 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);
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLEHOLD:
return (bool) FLIGHT_MODE(ANGLEHOLD_MODE);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR:

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

@ -157,7 +157,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
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLEHOLD, // 16
} logicFlightModeOperands_e;
typedef enum {

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

@ -352,8 +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";
} else if (FLIGHT_MODE(ANGLEHOLD_MODE)) {
flightMode = "AHLD";
}
#ifdef USE_GPS
} else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {

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

@ -191,7 +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_ANGLEHOLD: 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;
@ -221,7 +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_ANGLEHOLD: 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;