mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 00:35:34 +03:00
Merge branch 'iNavFlight:master' into master
This commit is contained in:
Julio Cesar Matias
GitHub
commit
1e4fdc562c
18 changed files with 265 additions and 174 deletions
127
src/main/fc/fc_core.c
Executable file → Normal file
127
src/main/fc/fc_core.c
Executable file → Normal file
|
|
@ -548,7 +548,7 @@ void tryArm(void)
|
|||
if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) {
|
||||
sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
|
||||
ENABLE_ARMING_FLAG(ARMED);
|
||||
enableFlightMode(TURTLE_MODE);
|
||||
ENABLE_FLIGHT_MODE(TURTLE_MODE);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
|
@ -678,28 +678,21 @@ void processRx(timeUs_t currentTimeUs)
|
|||
// Angle mode forced on briefly after emergency inflight rearm to help stabilise attitude (currently limited to MR)
|
||||
bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs);
|
||||
bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce;
|
||||
bool canUseHorizonMode = true;
|
||||
|
||||
if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) {
|
||||
// bumpless transfer to Level mode
|
||||
canUseHorizonMode = false;
|
||||
/* 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(ANGLEHOLD_MODE);
|
||||
|
||||
if (!FLIGHT_MODE(ANGLE_MODE)) {
|
||||
if (sensors(SENSOR_ACC) && (!FLIGHT_MODE(MANUAL_MODE) || autoEnableAngle)) {
|
||||
if (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle) {
|
||||
ENABLE_FLIGHT_MODE(ANGLE_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)) {
|
||||
} else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) {
|
||||
ENABLE_FLIGHT_MODE(HORIZON_MODE);
|
||||
} else if (STATE(AIRPLANE) && IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
|
||||
ENABLE_FLIGHT_MODE(ANGLEHOLD_MODE);
|
||||
}
|
||||
} else {
|
||||
DISABLE_FLIGHT_MODE(HORIZON_MODE);
|
||||
}
|
||||
|
||||
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
|
||||
|
|
@ -710,18 +703,14 @@ void processRx(timeUs_t currentTimeUs)
|
|||
|
||||
/* Flaperon mode */
|
||||
if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
|
||||
if (!FLIGHT_MODE(FLAPERON)) {
|
||||
ENABLE_FLIGHT_MODE(FLAPERON);
|
||||
}
|
||||
ENABLE_FLIGHT_MODE(FLAPERON);
|
||||
} else {
|
||||
DISABLE_FLIGHT_MODE(FLAPERON);
|
||||
}
|
||||
|
||||
/* Turn assistant mode */
|
||||
if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) {
|
||||
if (!FLIGHT_MODE(TURN_ASSISTANT)) {
|
||||
ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
|
||||
}
|
||||
ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
|
||||
} else {
|
||||
DISABLE_FLIGHT_MODE(TURN_ASSISTANT);
|
||||
}
|
||||
|
|
@ -740,9 +729,7 @@ void processRx(timeUs_t currentTimeUs)
|
|||
#if defined(USE_MAG)
|
||||
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
|
||||
if (IS_RC_MODE_ACTIVE(BOXHEADFREE) && STATE(MULTIROTOR)) {
|
||||
if (!FLIGHT_MODE(HEADFREE_MODE)) {
|
||||
ENABLE_FLIGHT_MODE(HEADFREE_MODE);
|
||||
}
|
||||
ENABLE_FLIGHT_MODE(HEADFREE_MODE);
|
||||
} else {
|
||||
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
|
||||
}
|
||||
|
|
@ -760,7 +747,7 @@ void processRx(timeUs_t currentTimeUs)
|
|||
} else {
|
||||
DISABLE_FLIGHT_MODE(MANUAL_MODE);
|
||||
}
|
||||
}else{
|
||||
} else {
|
||||
DISABLE_FLIGHT_MODE(MANUAL_MODE);
|
||||
}
|
||||
|
||||
|
|
@ -781,52 +768,52 @@ void processRx(timeUs_t currentTimeUs)
|
|||
}
|
||||
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
|
||||
if (throttleIsLow) {
|
||||
if (STATE(AIRMODE_ACTIVE)) {
|
||||
if ((rollPitchStatus == CENTERED) || (ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY))) {
|
||||
ENABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
pidResetErrorAccumulators();
|
||||
}
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
if (STATE(AIRMODE_ACTIVE)) {
|
||||
if ((rollPitchStatus == CENTERED) || (ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY))) {
|
||||
ENABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
pidResetErrorAccumulators();
|
||||
}
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
}
|
||||
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) {
|
||||
if (throttleIsLow) {
|
||||
if (STATE(AIRMODE_ACTIVE)) {
|
||||
if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) {
|
||||
ENABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
pidResetErrorAccumulators();
|
||||
}
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
if (rollPitchStatus != CENTERED) {
|
||||
ENABLE_STATE(ANTI_WINDUP_DEACTIVATED);
|
||||
}
|
||||
}
|
||||
if (STATE(AIRMODE_ACTIVE)) {
|
||||
if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) {
|
||||
ENABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
pidResetErrorAccumulators();
|
||||
}
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
if (rollPitchStatus != CENTERED) {
|
||||
ENABLE_STATE(ANTI_WINDUP_DEACTIVATED);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
//This case applies only to MR when Airmode management is throttle threshold activated
|
||||
if (throttleIsLow && !STATE(AIRMODE_ACTIVE)) {
|
||||
pidResetErrorAccumulators();
|
||||
}
|
||||
}
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
//This case applies only to MR when Airmode management is throttle threshold activated
|
||||
if (throttleIsLow && !STATE(AIRMODE_ACTIVE)) {
|
||||
pidResetErrorAccumulators();
|
||||
}
|
||||
}
|
||||
//---------------------------------------------------------
|
||||
if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) {
|
||||
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
|
||||
|
|
@ -849,6 +836,8 @@ void processRx(timeUs_t currentTimeUs)
|
|||
}
|
||||
}
|
||||
#endif
|
||||
// Sound a beeper if the flight mode state has changed
|
||||
updateFlightModeChangeBeeper();
|
||||
}
|
||||
|
||||
// Function for loop trigger
|
||||
|
|
|
|||
|
|
@ -102,6 +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 = BOXANGLEHOLD, .boxName = "ANGLE HOLD", .permanentId = 64 },
|
||||
{ .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF }
|
||||
};
|
||||
|
||||
|
|
@ -259,7 +260,7 @@ void initActiveBoxIds(void)
|
|||
ADD_ACTIVE_BOX(BOXNAVALTHOLD);
|
||||
}
|
||||
|
||||
if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT) ||
|
||||
if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT) ||
|
||||
platformTypeConfigured(PLATFORM_AIRPLANE) || platformTypeConfigured(PLATFORM_ROVER) || platformTypeConfigured(PLATFORM_BOAT)) {
|
||||
ADD_ACTIVE_BOX(BOXMANUAL);
|
||||
}
|
||||
|
|
@ -279,6 +280,9 @@ void initActiveBoxIds(void)
|
|||
if (sensors(SENSOR_BARO)) {
|
||||
ADD_ACTIVE_BOX(BOXAUTOLEVEL);
|
||||
}
|
||||
if (sensors(SENSOR_ACC)) {
|
||||
ADD_ACTIVE_BOX(BOXANGLEHOLD);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -432,6 +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(BOXANGLEHOLD)), BOXANGLEHOLD);
|
||||
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
|
||||
for (uint32_t i = 0; i < activeBoxIdCount; i++) {
|
||||
if (activeBoxes[activeBoxIds[i]]) {
|
||||
|
|
|
|||
|
|
@ -81,6 +81,7 @@ typedef enum {
|
|||
BOXMULTIFUNCTION = 52,
|
||||
BOXMIXERPROFILE = 53,
|
||||
BOXMIXERTRANSITION = 54,
|
||||
BOXANGLEHOLD = 55,
|
||||
CHECKBOX_ITEM_COUNT
|
||||
} boxId_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -91,31 +91,16 @@ armingFlag_e isArmingDisabledReason(void)
|
|||
}
|
||||
|
||||
/**
|
||||
* Enables the given flight mode. A beep is sounded if the flight mode
|
||||
* has changed. Returns the new 'flightModeFlags' value.
|
||||
* Called at Rx update rate. Beeper sounded if flight mode state has changed.
|
||||
*/
|
||||
uint32_t enableFlightMode(flightModeFlags_e mask)
|
||||
void updateFlightModeChangeBeeper(void)
|
||||
{
|
||||
uint32_t oldVal = flightModeFlags;
|
||||
static uint32_t previousFlightModeFlags = 0;
|
||||
|
||||
flightModeFlags |= (mask);
|
||||
if (flightModeFlags != oldVal)
|
||||
if (flightModeFlags != previousFlightModeFlags) {
|
||||
beeperConfirmationBeeps(1);
|
||||
return flightModeFlags;
|
||||
}
|
||||
|
||||
/**
|
||||
* Disables the given flight mode. A beep is sounded if the flight mode
|
||||
* has changed. Returns the new 'flightModeFlags' value.
|
||||
*/
|
||||
uint32_t disableFlightMode(flightModeFlags_e mask)
|
||||
{
|
||||
uint32_t oldVal = flightModeFlags;
|
||||
|
||||
flightModeFlags &= ~(mask);
|
||||
if (flightModeFlags != oldVal)
|
||||
beeperConfirmationBeeps(1);
|
||||
return flightModeFlags;
|
||||
}
|
||||
previousFlightModeFlags = flightModeFlags;
|
||||
}
|
||||
|
||||
bool sensors(uint32_t mask)
|
||||
|
|
@ -173,6 +158,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
|
|||
if (FLIGHT_MODE(HORIZON_MODE))
|
||||
return FLM_HORIZON;
|
||||
|
||||
if (FLIGHT_MODE(ANGLEHOLD_MODE))
|
||||
return FLM_ANGLEHOLD;
|
||||
|
||||
return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -104,12 +104,13 @@ typedef enum {
|
|||
TURN_ASSISTANT = (1 << 14),
|
||||
TURTLE_MODE = (1 << 15),
|
||||
SOARING_MODE = (1 << 16),
|
||||
ANGLEHOLD_MODE = (1 << 17),
|
||||
} flightModeFlags_e;
|
||||
|
||||
extern uint32_t flightModeFlags;
|
||||
|
||||
#define DISABLE_FLIGHT_MODE(mask) disableFlightMode(mask)
|
||||
#define ENABLE_FLIGHT_MODE(mask) enableFlightMode(mask)
|
||||
#define DISABLE_FLIGHT_MODE(mask) (flightModeFlags &= ~(mask))
|
||||
#define ENABLE_FLIGHT_MODE(mask) (flightModeFlags |= (mask))
|
||||
#define FLIGHT_MODE(mask) (flightModeFlags & (mask))
|
||||
|
||||
typedef enum {
|
||||
|
|
@ -162,6 +163,7 @@ typedef enum {
|
|||
FLM_CRUISE,
|
||||
FLM_LAUNCH,
|
||||
FLM_FAILSAFE,
|
||||
FLM_ANGLEHOLD,
|
||||
FLM_COUNT
|
||||
} flightModeForTelemetry_e;
|
||||
|
||||
|
|
@ -200,8 +202,7 @@ extern simulatorData_t simulatorData;
|
|||
|
||||
#endif
|
||||
|
||||
uint32_t enableFlightMode(flightModeFlags_e mask);
|
||||
uint32_t disableFlightMode(flightModeFlags_e mask);
|
||||
void updateFlightModeChangeBeeper(void);
|
||||
|
||||
bool sensors(uint32_t mask);
|
||||
void sensorsSet(uint32_t mask);
|
||||
|
|
|
|||
|
|
@ -192,7 +192,7 @@ tables:
|
|||
enum: gpsBaudRate_e
|
||||
- name: nav_mc_althold_throttle
|
||||
values: ["STICK", "MID_STICK", "HOVER"]
|
||||
enum: navMcAltHoldThrottle_e
|
||||
enum: navMcAltHoldThrottle_e
|
||||
- name: led_pin_pwm_mode
|
||||
values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"]
|
||||
enum: led_pin_pwm_mode_e
|
||||
|
|
@ -1186,7 +1186,7 @@ groups:
|
|||
min: 0
|
||||
max: 200
|
||||
|
||||
|
||||
|
||||
|
||||
- name: PG_REVERSIBLE_MOTORS_CONFIG
|
||||
type: reversibleMotorsConfig_t
|
||||
|
|
@ -3586,7 +3586,7 @@ groups:
|
|||
min: 1000
|
||||
max: 5000
|
||||
default_value: 1500
|
||||
|
||||
|
||||
- name: osd_switch_indicator_zero_name
|
||||
description: "Character to use for OSD switch incicator 0."
|
||||
field: osd_switch_indicator0_name
|
||||
|
|
|
|||
|
|
@ -159,6 +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 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
|
||||
|
|
@ -838,7 +840,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
|
|||
|
||||
pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT)
|
||||
+ ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);
|
||||
|
||||
|
||||
if (pidProfile()->pidItermLimitPercent != 0){
|
||||
float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f;
|
||||
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
|
||||
|
|
@ -1064,9 +1066,62 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
|
|||
|
||||
}
|
||||
|
||||
bool isAngleHoldLevel(void)
|
||||
{
|
||||
return angleHoldIsLevel;
|
||||
}
|
||||
|
||||
void updateAngleHold(float *angleTarget, uint8_t axis)
|
||||
{
|
||||
int8_t navAngleHoldAxis = navCheckActiveAngleHoldAxis();
|
||||
|
||||
if (!restartAngleHoldMode) { // set restart flag when anglehold is inactive
|
||||
restartAngleHoldMode = !FLIGHT_MODE(ANGLEHOLD_MODE) && navAngleHoldAxis == -1;
|
||||
}
|
||||
|
||||
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 angleHoldTarget[2];
|
||||
|
||||
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 anglehold is level
|
||||
if (FLIGHT_MODE(ANGLEHOLD_MODE)) {
|
||||
angleHoldIsLevel = angleHoldTarget[FD_ROLL] == 0 && angleHoldTarget[FD_PITCH] == 0;
|
||||
} else {
|
||||
angleHoldIsLevel = angleHoldTarget[navAngleHoldAxis] == 0;
|
||||
}
|
||||
|
||||
uint16_t bankLimit = pidProfile()->max_angle_inclination[axis];
|
||||
|
||||
// use Nav bank angle limits if Nav active
|
||||
if (navAngleHoldAxis == FD_ROLL) {
|
||||
bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle);
|
||||
} 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) {
|
||||
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);
|
||||
angleHoldTarget[axis] = attitude.raw[axis] + levelTrim;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void FAST_CODE pidController(float dT)
|
||||
{
|
||||
|
||||
const float dT_inv = 1.0f / dT;
|
||||
|
||||
if (!pidFiltersConfigured) {
|
||||
|
|
@ -1115,21 +1170,30 @@ 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 ANGLEHOLD_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) || isFlightAxisAngleOverrideActive(axis)) {
|
||||
//If axis angle override, get the correct angle from Logic Conditions
|
||||
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));
|
||||
|
||||
//Apply the Level PID controller
|
||||
if (STATE(AIRPLANE)) { // update anglehold mode
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
// 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]));
|
||||
|
|
@ -1137,6 +1201,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 && STATE(MULTIROTOR)) {
|
||||
pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
|
||||
}
|
||||
|
|
@ -1272,7 +1337,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();
|
||||
!navigationIsControllingAltitude() && !(navCheckActiveAngleHoldAxis() == FD_PITCH && !angleHoldIsLevel);
|
||||
}
|
||||
|
||||
void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
|
||||
|
|
|
|||
|
|
@ -220,3 +220,4 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex);
|
|||
bool isFixedWingLevelTrimActive(void);
|
||||
void updateFixedWingLevelTrim(timeUs_t currentTimeUs);
|
||||
float getFixedWingLevelTrim(void);
|
||||
bool isAngleHoldLevel(void);
|
||||
|
|
|
|||
|
|
@ -1726,7 +1726,7 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
tfp_sprintf(buff, "%5d", (int)getMAhDrawn()); // Use 5 digits to allow packs below 100Ah
|
||||
buff[5] = SYM_MAH;
|
||||
buff[6] = '\0';
|
||||
} else
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (mah_digits - 2), mah_digits, false)) {
|
||||
|
|
@ -1760,7 +1760,7 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
tfp_sprintf(buff, " NF");
|
||||
else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) {
|
||||
uint8_t mah_digits = osdConfig()->mAh_precision; // Initialize to config value
|
||||
|
||||
|
||||
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
|
||||
if (isBfCompatibleVideoSystem(osdConfig())) {
|
||||
//BFcompat is unable to work with scaled values and it only has mAh symbol to work with
|
||||
|
|
@ -1768,7 +1768,7 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
buff[5] = SYM_MAH;
|
||||
buff[6] = '\0';
|
||||
unitsDrawn = true;
|
||||
} else
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
if (osdFormatCentiNumber(buff, getBatteryRemainingCapacity() * 100, 1000, 0, (mah_digits - 2), mah_digits, false)) {
|
||||
|
|
@ -2258,7 +2258,7 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
p = " WP ";
|
||||
else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
|
||||
// If navigationRequiresAngleMode() returns false when ALTHOLD is active,
|
||||
// it means it can be combined with ANGLE, HORIZON, ACRO, etc...
|
||||
// it means it can be combined with ANGLE, HORIZON, ANGLEHOLD, ACRO, etc...
|
||||
// and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE.
|
||||
p = " AH ";
|
||||
}
|
||||
|
|
@ -2266,6 +2266,8 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
p = "ANGL";
|
||||
else if (FLIGHT_MODE(HORIZON_MODE))
|
||||
p = "HOR ";
|
||||
else if (FLIGHT_MODE(ANGLEHOLD_MODE))
|
||||
p = "ANGH";
|
||||
|
||||
displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
|
||||
return true;
|
||||
|
|
@ -5156,10 +5158,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
|
|||
}
|
||||
} else {
|
||||
if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
|
||||
// ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO
|
||||
// ALTHOLD might be enabled alongside ANGLE/HORIZON/ANGLEHOLD/ACRO
|
||||
// when it doesn't require ANGLE mode (required only in FW
|
||||
// right now). If if requires ANGLE, its display is handled
|
||||
// by OSD_FLYMODE.
|
||||
// right now). If it requires ANGLE, its display is handled by OSD_FLYMODE.
|
||||
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
|
||||
}
|
||||
if (STATE(MULTIROTOR) && FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
|
|
@ -5196,6 +5197,16 @@ 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(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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
} else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
|
||||
|
|
|
|||
|
|
@ -118,6 +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_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"
|
||||
|
|
@ -413,7 +416,7 @@ typedef struct osdConfig_s {
|
|||
|
||||
#ifdef USE_WIND_ESTIMATOR
|
||||
bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance
|
||||
#endif
|
||||
#endif
|
||||
uint8_t coordinate_digits;
|
||||
bool osd_failsafe_switch_layout;
|
||||
uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE
|
||||
|
|
|
|||
|
|
@ -300,6 +300,7 @@ static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
|
|||
case FLM_ALTITUDE_HOLD:
|
||||
case FLM_POSITION_HOLD:
|
||||
case FLM_MISSION:
|
||||
case FLM_ANGLEHOLD:
|
||||
default:
|
||||
// Unsupported ATM, keep at ANGLE
|
||||
bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE
|
||||
|
|
|
|||
|
|
@ -3894,9 +3894,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
/* Soaring mode, disables altitude control in Position hold and Course hold modes.
|
||||
* Pitch allowed to freefloat within defined Angle mode deadband */
|
||||
if (IS_RC_MODE_ACTIVE(BOXSOARING) && (FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_COURSE_HOLD_MODE))) {
|
||||
if (!FLIGHT_MODE(SOARING_MODE)) {
|
||||
ENABLE_FLIGHT_MODE(SOARING_MODE);
|
||||
}
|
||||
ENABLE_FLIGHT_MODE(SOARING_MODE);
|
||||
} else {
|
||||
DISABLE_FLIGHT_MODE(SOARING_MODE);
|
||||
}
|
||||
|
|
@ -4567,3 +4565,21 @@ int32_t navigationGetHeadingError(void)
|
|||
{
|
||||
return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog);
|
||||
}
|
||||
|
||||
int8_t navCheckActiveAngleHoldAxis(void)
|
||||
{
|
||||
int8_t activeAxis = -1;
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
|
||||
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
|
||||
bool altholdActive = stateFlags & NAV_REQUIRE_ANGLE_FW && !(stateFlags & NAV_REQUIRE_ANGLE);
|
||||
|
||||
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
|
||||
activeAxis = FD_PITCH;
|
||||
} else if (altholdActive) {
|
||||
activeAxis = FD_ROLL;
|
||||
}
|
||||
}
|
||||
|
||||
return activeAxis;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -627,6 +627,8 @@ bool isEstimatedAglTrusted(void);
|
|||
void checkManualEmergencyLandingControl(bool forcedActivation);
|
||||
float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue);
|
||||
|
||||
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
|
||||
* are in the [0, 360 * 100) interval.
|
||||
|
|
|
|||
|
|
@ -97,7 +97,7 @@ static int logicConditionCompute(
|
|||
int temporaryValue;
|
||||
#if defined(USE_VTX_CONTROL)
|
||||
vtxDeviceCapability_t vtxDeviceCapability;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
switch (operation) {
|
||||
|
||||
|
|
@ -154,7 +154,7 @@ static int logicConditionCompute(
|
|||
|
||||
case LOGIC_CONDITION_NOR:
|
||||
return !(operandA || operandB);
|
||||
break;
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_NOT:
|
||||
return !operandA;
|
||||
|
|
@ -170,7 +170,7 @@ static int logicConditionCompute(
|
|||
return false;
|
||||
}
|
||||
|
||||
//When both operands are not met, keep current value
|
||||
//When both operands are not met, keep current value
|
||||
return currentValue;
|
||||
break;
|
||||
|
||||
|
|
@ -238,7 +238,7 @@ static int logicConditionCompute(
|
|||
gvSet(operandA, operandB);
|
||||
return operandB;
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_GVAR_INC:
|
||||
temporaryValue = gvGet(operandA) + operandB;
|
||||
gvSet(operandA, temporaryValue);
|
||||
|
|
@ -270,7 +270,7 @@ static int logicConditionCompute(
|
|||
return operandA;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY:
|
||||
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY);
|
||||
return true;
|
||||
|
|
@ -288,10 +288,10 @@ static int logicConditionCompute(
|
|||
break;
|
||||
|
||||
case LOGIC_CONDITION_SET_VTX_POWER_LEVEL:
|
||||
#if defined(USE_VTX_CONTROL)
|
||||
#if defined(USE_VTX_CONTROL)
|
||||
#if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP))
|
||||
if (
|
||||
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA &&
|
||||
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA &&
|
||||
vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
|
||||
) {
|
||||
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
|
||||
|
|
@ -339,18 +339,18 @@ static int logicConditionCompute(
|
|||
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH);
|
||||
return true;
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_INVERT_YAW:
|
||||
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW);
|
||||
return true;
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_OVERRIDE_THROTTLE:
|
||||
logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE] = operandA;
|
||||
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE);
|
||||
return operandA;
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_SET_OSD_LAYOUT:
|
||||
logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT] = operandA;
|
||||
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT);
|
||||
|
|
@ -366,18 +366,18 @@ static int logicConditionCompute(
|
|||
|
||||
case LOGIC_CONDITION_SIN:
|
||||
temporaryValue = (operandB == 0) ? 500 : operandB;
|
||||
return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
|
||||
return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_COS:
|
||||
temporaryValue = (operandB == 0) ? 500 : operandB;
|
||||
return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
|
||||
return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
|
||||
break;
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_TAN:
|
||||
temporaryValue = (operandB == 0) ? 500 : operandB;
|
||||
return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
|
||||
return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_MIN:
|
||||
|
|
@ -387,11 +387,11 @@ static int logicConditionCompute(
|
|||
case LOGIC_CONDITION_MAX:
|
||||
return (operandA > operandB) ? operandA : operandB;
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_MAP_INPUT:
|
||||
return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000);
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_MAP_OUTPUT:
|
||||
return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB);
|
||||
break;
|
||||
|
|
@ -474,7 +474,7 @@ static int logicConditionCompute(
|
|||
} else {
|
||||
return false;
|
||||
}
|
||||
break;
|
||||
break;
|
||||
|
||||
#ifdef LED_PIN
|
||||
case LOGIC_CONDITION_LED_PIN_PWM:
|
||||
|
|
@ -486,10 +486,9 @@ static int logicConditionCompute(
|
|||
return operandA;
|
||||
break;
|
||||
#endif
|
||||
|
||||
default:
|
||||
return false;
|
||||
break;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -498,7 +497,7 @@ void logicConditionProcess(uint8_t i) {
|
|||
const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId);
|
||||
|
||||
if (logicConditions(i)->enabled && activatorValue && !cliMode) {
|
||||
|
||||
|
||||
/*
|
||||
* Process condition only when latch flag is not set
|
||||
* Latched LCs can only go from OFF to ON, not the other way
|
||||
|
|
@ -507,13 +506,13 @@ void logicConditionProcess(uint8_t i) {
|
|||
const int operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value);
|
||||
const int operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value);
|
||||
const int newValue = logicConditionCompute(
|
||||
logicConditionStates[i].value,
|
||||
logicConditions(i)->operation,
|
||||
operandAValue,
|
||||
logicConditionStates[i].value,
|
||||
logicConditions(i)->operation,
|
||||
operandAValue,
|
||||
operandBValue,
|
||||
i
|
||||
);
|
||||
|
||||
|
||||
logicConditionStates[i].value = newValue;
|
||||
|
||||
/*
|
||||
|
|
@ -588,7 +587,7 @@ static int logicConditionGetWaypointOperandValue(int operand) {
|
|||
return distance;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION:
|
||||
return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0;
|
||||
break;
|
||||
|
|
@ -634,11 +633,11 @@ static int logicConditionGetFlightOperandValue(int operand) {
|
|||
case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s
|
||||
return constrain((uint32_t)getFlightTime(), 0, INT16_MAX);
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m
|
||||
return constrain(GPS_distanceToHome, 0, INT16_MAX);
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m
|
||||
return constrain(getTotalTravelDistance() / 100, 0, INT16_MAX);
|
||||
break;
|
||||
|
|
@ -646,7 +645,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
|
|||
case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI:
|
||||
return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100
|
||||
return getBatteryVoltage();
|
||||
break;
|
||||
|
|
@ -662,7 +661,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
|
|||
case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100
|
||||
return getAmperage();
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh
|
||||
return getMAhDrawn();
|
||||
break;
|
||||
|
|
@ -674,7 +673,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
|
|||
return gpsSol.numSat;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1
|
||||
return STATE(GPS_FIX) ? 1 : 0;
|
||||
break;
|
||||
|
|
@ -726,15 +725,15 @@ static int logicConditionGetFlightOperandValue(int operand) {
|
|||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1
|
||||
return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0;
|
||||
break;
|
||||
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1
|
||||
return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0;
|
||||
break;
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1
|
||||
return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0;
|
||||
break;
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1
|
||||
return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0;
|
||||
|
|
@ -742,7 +741,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
|
|||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1
|
||||
return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0;
|
||||
break;
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
|
||||
return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0;
|
||||
|
|
@ -751,16 +750,16 @@ static int logicConditionGetFlightOperandValue(int operand) {
|
|||
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1
|
||||
return (failsafePhase() != FAILSAFE_IDLE) ? 1 : 0;
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: //
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: //
|
||||
return axisPID[YAW];
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: //
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: //
|
||||
return axisPID[ROLL];
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: //
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: //
|
||||
return axisPID[PITCH];
|
||||
break;
|
||||
|
||||
|
|
@ -787,7 +786,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
|
|||
case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int
|
||||
return getConfigProfile() + 1;
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int
|
||||
return currentMixerProfileIndex + 1;
|
||||
break;
|
||||
|
|
@ -802,14 +801,14 @@ static int logicConditionGetFlightOperandValue(int operand) {
|
|||
case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS:
|
||||
return isEstimatedAglTrusted();
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_AGL:
|
||||
return getEstimatedAglPosition();
|
||||
break;
|
||||
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW:
|
||||
return rangefinderGetLatestRawAltitude();
|
||||
break;
|
||||
break;
|
||||
|
||||
default:
|
||||
return 0;
|
||||
|
|
@ -861,13 +860,18 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
|
|||
return (bool) FLIGHT_MODE(HORIZON_MODE);
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLEHOLD:
|
||||
return (bool) FLIGHT_MODE(ANGLEHOLD_MODE);
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR:
|
||||
return (bool) FLIGHT_MODE(AIRMODE_ACTIVE);
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO:
|
||||
return (((bool) FLIGHT_MODE(ANGLE_MODE) || (bool) FLIGHT_MODE(HORIZON_MODE) || (bool) FLIGHT_MODE(MANUAL_MODE) || (bool) FLIGHT_MODE(NAV_RTH_MODE) ||
|
||||
(bool) FLIGHT_MODE(NAV_POSHOLD_MODE) || (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (bool) FLIGHT_MODE(NAV_WP_MODE)) == false);
|
||||
return (((bool) FLIGHT_MODE(ANGLE_MODE) || (bool) FLIGHT_MODE(HORIZON_MODE) || (bool) FLIGHT_MODE(ANGLEHOLD_MODE) ||
|
||||
(bool) FLIGHT_MODE(MANUAL_MODE) || (bool) FLIGHT_MODE(NAV_RTH_MODE) || (bool) FLIGHT_MODE(NAV_POSHOLD_MODE) ||
|
||||
(bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (bool) FLIGHT_MODE(NAV_WP_MODE)) == false);
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1:
|
||||
|
|
@ -905,7 +909,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
|
|||
//Extract RC channel raw value
|
||||
if (operand >= 1 && operand <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
|
||||
retVal = rxGetChannelValue(operand - 1);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT:
|
||||
|
|
@ -933,7 +937,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
|
|||
retVal = programmingPidGetOutput(operand);
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS:
|
||||
retVal = logicConditionGetWaypointOperandValue(operand);
|
||||
break;
|
||||
|
|
@ -947,7 +951,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
|
|||
|
||||
/*
|
||||
* conditionId == -1 is always evaluated as true
|
||||
*/
|
||||
*/
|
||||
int logicConditionGetValue(int8_t conditionId) {
|
||||
if (conditionId >= 0) {
|
||||
return logicConditionStates[conditionId].value;
|
||||
|
|
@ -1029,7 +1033,7 @@ int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) {
|
|||
|
||||
uint32_t getLoiterRadius(uint32_t loiterRadius) {
|
||||
#ifdef USE_PROGRAMMING_FRAMEWORK
|
||||
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) &&
|
||||
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) &&
|
||||
!(FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding())) {
|
||||
return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000);
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -158,6 +158,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_ANGLEHOLD, // 16
|
||||
} logicFlightModeOperands_e;
|
||||
|
||||
typedef enum {
|
||||
|
|
|
|||
|
|
@ -352,6 +352,8 @@ static void crsfFrameFlightMode(sbuf_t *dst)
|
|||
flightMode = "ANGL";
|
||||
} else if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
flightMode = "HOR";
|
||||
} else if (FLIGHT_MODE(ANGLEHOLD_MODE)) {
|
||||
flightMode = "ANGH";
|
||||
}
|
||||
#ifdef USE_GPS
|
||||
} else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {
|
||||
|
|
|
|||
|
|
@ -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_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;
|
||||
|
|
@ -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_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;
|
||||
|
|
@ -662,7 +664,7 @@ void mavlinkSendHUDAndHeartbeat(void)
|
|||
// heading Current heading in degrees, in compass units (0..360, 0=north)
|
||||
DECIDEGREES_TO_DEGREES(attitude.values.yaw),
|
||||
// throttle Current throttle setting in integer percent, 0 to 100
|
||||
thr,
|
||||
thr,
|
||||
// alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate)
|
||||
mavAltitude,
|
||||
// climb Current climb rate in meters/second
|
||||
|
|
@ -1102,9 +1104,9 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
|
|||
|
||||
if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) {
|
||||
// Only process scheduled data if we didn't serve any incoming request this cycle
|
||||
if (!incomingRequestServed ||
|
||||
if (!incomingRequestServed ||
|
||||
(
|
||||
(rxConfig()->receiverType == RX_TYPE_SERIAL) &&
|
||||
(rxConfig()->receiverType == RX_TYPE_SERIAL) &&
|
||||
(rxConfig()->serialrx_provider == SERIALRX_MAVLINK) &&
|
||||
!tristateWithDefaultOnIsActive(rxConfig()->halfDuplex)
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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", "ANH" };
|
||||
static const char gpsFixIndicators[] = { '!', '*', ' ' };
|
||||
|
||||
static bool checkGroundStationNumber(uint8_t* rv)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue