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

first build

Browse source
This commit is contained in:
breadoven 2023-09-13 21:48:35 +01:00
parent 0d8e7889ca
commit bba9efa98c
7 changed files with 64 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -644,28 +644,25 @@ void processRx(timeUs_t currentTimeUs)
processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData);
}
bool canUseHorizonMode = true;
/* 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 ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) && sensors(SENSOR_ACC)) {
// bumpless transfer to Level mode
canUseHorizonMode = false;
if (!FLIGHT_MODE(ANGLE_MODE)) {
ENABLE_FLIGHT_MODE(ANGLE_MODE);
if (sensors(SENSOR_ACC)) {
if (IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) {
if (!FLIGHT_MODE(ANGLE_MODE)) {
ENABLE_FLIGHT_MODE(ANGLE_MODE);
}
} else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) {
if (!FLIGHT_MODE(HORIZON_MODE)) {
ENABLE_FLIGHT_MODE(HORIZON_MODE);
}
} else if (STATE(AIRPLANE) && IS_RC_MODE_ACTIVE(BOXATTIHOLD)) {
if (!FLIGHT_MODE(ATTIHOLD_MODE)) {
ENABLE_FLIGHT_MODE(ATTIHOLD_MODE);
}
}
} else {
DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
}
if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
DISABLE_FLIGHT_MODE(ANGLE_MODE);
if (!FLIGHT_MODE(HORIZON_MODE)) {
ENABLE_FLIGHT_MODE(HORIZON_MODE);
}
} else {
DISABLE_FLIGHT_MODE(HORIZON_MODE);
}
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_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;

1
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;

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

@ -1107,21 +1107,52 @@ void FAST_CODE pidController(float dT)
#endif
}
// Step 3: Run control for ANGLE_MODE, HORIZON_MODE, and HEADING_LOCK
const float horizonRateMagnitude = calcHorizonRateMagnitude();
// Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ATTI_MODE
levelingEnabled = false;
static bool restartAttiMode = true;
bool attiModeActive = false;
for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || isFlightAxisAngleOverrideActive(axis)) {
//If axis angle override, get the correct angle from Logic Conditions
const float horizonRateMagnitude = calcHorizonRateMagnitude();
// If axis angle override, get the correct angle from Logic Conditions
float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis));
//Apply the Level PID controller
// Apply the Level PID controller
pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON
levelingEnabled = true;
}
else if (FLIGHT_MODE(ATTIHOLD_MODE)) {
static int16_t attiHoldTarget[2];
attiModeActive = true;
// set target attitude to current attitude when initialised
if (restartAttiMode) {
attiHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
attiHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH];
restartAttiMode = false;
}
uint16_t attiAngleTarget;
uint16_t bankLimit = pidProfile()->max_angle_inclination[axis];
if (calculateRollPitchCenterStatus() == CENTERED) {
attiAngleTarget = constrain(attiHoldTarget[axis], -bankLimit, bankLimit);
} else {
attiAngleTarget = constrain(attitude.raw[axis] + (bankLimit * rcCommand[axis] / 500), -bankLimit, bankLimit);
attiHoldTarget[axis] = attitude.raw[axis];
}
pidLevel(attiAngleTarget, &pidState[axis], axis, 0, dT);
}
}
// set restart flag if attihold_mode not active on previous loop
if (!restartAttiMode) {
restartAttiMode = !attiModeActive;
}
// apply Turn Assistance
if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
@ -1129,6 +1160,7 @@ void FAST_CODE pidController(float dT)
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT
}
// apply FPV camera mix
if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees) {
pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
}

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

@ -2208,6 +2208,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;

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))) {