mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-12 19:10:27 +03:00
1114 lines
39 KiB
C
1114 lines
39 KiB
C
/*
|
|
* This file is part of INAV Project.
|
|
*
|
|
* This Source Code Form is subject to the terms of the Mozilla Public
|
|
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
|
|
* You can obtain one at http://mozilla.org/MPL/2.0/.
|
|
*
|
|
* Alternatively, the contents of this file may be used under the terms
|
|
* of the GNU General Public License Version 3, as described below:
|
|
*
|
|
* This file is free software: you may copy, redistribute and/or modify
|
|
* it under the terms of the GNU General Public License as published by the
|
|
* Free Software Foundation, either version 3 of the License, or (at your
|
|
* option) any later version.
|
|
*
|
|
* This file is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
|
|
* Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program. If not, see http://www.gnu.org/licenses/.
|
|
*/
|
|
|
|
#include <stdbool.h>
|
|
|
|
#include "config/config_reset.h"
|
|
#include "config/parameter_group.h"
|
|
#include "config/parameter_group_ids.h"
|
|
|
|
#include "programming/logic_condition.h"
|
|
#include "programming/global_variables.h"
|
|
#include "programming/pid.h"
|
|
#include "common/utils.h"
|
|
#include "rx/rx.h"
|
|
#include "common/maths.h"
|
|
#include "fc/config.h"
|
|
#include "fc/cli.h"
|
|
#include "fc/fc_core.h"
|
|
#include "fc/rc_controls.h"
|
|
#include "fc/runtime_config.h"
|
|
#include "fc/rc_modes.h"
|
|
#include "navigation/navigation.h"
|
|
#include "sensors/battery.h"
|
|
#include "sensors/pitotmeter.h"
|
|
#include "sensors/rangefinder.h"
|
|
#include "flight/imu.h"
|
|
#include "flight/pid.h"
|
|
#include "flight/mixer_profile.h"
|
|
#include "drivers/io_port_expander.h"
|
|
#include "io/osd_common.h"
|
|
#include "sensors/diagnostics.h"
|
|
|
|
#include "navigation/navigation.h"
|
|
#include "navigation/navigation_private.h"
|
|
|
|
#include "io/vtx.h"
|
|
#include "drivers/vtx_common.h"
|
|
#include "drivers/light_ws2811strip.h"
|
|
|
|
PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 4);
|
|
|
|
EXTENDED_FASTRAM uint64_t logicConditionsGlobalFlags;
|
|
EXTENDED_FASTRAM int logicConditionValuesByType[LOGIC_CONDITION_LAST];
|
|
EXTENDED_FASTRAM rcChannelOverride_t rcChannelOverrides[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
|
EXTENDED_FASTRAM flightAxisOverride_t flightAxisOverride[XYZ_AXIS_COUNT];
|
|
|
|
void pgResetFn_logicConditions(logicCondition_t *instance)
|
|
{
|
|
for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
|
|
RESET_CONFIG(logicCondition_t, &instance[i],
|
|
.enabled = 0,
|
|
.activatorId = -1,
|
|
.operation = 0,
|
|
.operandA = {
|
|
.type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
|
|
.value = 0
|
|
},
|
|
.operandB = {
|
|
.type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
|
|
.value = 0
|
|
},
|
|
.flags = 0
|
|
);
|
|
}
|
|
}
|
|
|
|
logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
|
|
|
|
static int logicConditionCompute(
|
|
int32_t currentValue,
|
|
logicOperation_e operation,
|
|
int32_t operandA,
|
|
int32_t operandB,
|
|
uint8_t lcIndex
|
|
) {
|
|
int temporaryValue;
|
|
#if defined(USE_VTX_CONTROL)
|
|
vtxDeviceCapability_t vtxDeviceCapability;
|
|
#endif
|
|
|
|
switch (operation) {
|
|
|
|
case LOGIC_CONDITION_TRUE:
|
|
return true;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_EQUAL:
|
|
return operandA == operandB;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_APPROX_EQUAL:
|
|
{
|
|
uint16_t offest = operandA / 100;
|
|
return ((operandB >= (operandA - offest)) && (operandB <= (operandA + offest)));
|
|
}
|
|
break;
|
|
|
|
case LOGIC_CONDITION_GREATER_THAN:
|
|
return operandA > operandB;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_LOWER_THAN:
|
|
return operandA < operandB;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_LOW:
|
|
return operandA < 1333;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_MID:
|
|
return operandA >= 1333 && operandA <= 1666;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_HIGH:
|
|
return operandA > 1666;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_AND:
|
|
return (operandA && operandB);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OR:
|
|
return (operandA || operandB);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_XOR:
|
|
return (operandA != operandB);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_NAND:
|
|
return !(operandA && operandB);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_NOR:
|
|
return !(operandA || operandB);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_NOT:
|
|
return !operandA;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_STICKY:
|
|
// Operand A is activation operator
|
|
if (operandA) {
|
|
return true;
|
|
}
|
|
//Operand B is deactivation operator
|
|
if (operandB) {
|
|
return false;
|
|
}
|
|
|
|
//When both operands are not met, keep current value
|
|
return currentValue;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_EDGE:
|
|
if (operandA && logicConditionStates[lcIndex].timeout == 0 && !(logicConditionStates[lcIndex].flags & LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED)) {
|
|
if (operandB < 100) {
|
|
logicConditionStates[lcIndex].timeout = millis();
|
|
} else {
|
|
logicConditionStates[lcIndex].timeout = millis() + operandB;
|
|
}
|
|
logicConditionStates[lcIndex].flags |= LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
|
|
return true;
|
|
} else if (logicConditionStates[lcIndex].timeout > 0) {
|
|
if (logicConditionStates[lcIndex].timeout < millis()) {
|
|
logicConditionStates[lcIndex].timeout = 0;
|
|
} else {
|
|
return true;
|
|
}
|
|
}
|
|
|
|
if (!operandA) {
|
|
logicConditionStates[lcIndex].flags &= ~LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
|
|
}
|
|
return false;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_DELAY:
|
|
if (operandA) {
|
|
if (logicConditionStates[lcIndex].timeout == 0) {
|
|
logicConditionStates[lcIndex].timeout = millis() + operandB;
|
|
} else if (millis() > logicConditionStates[lcIndex].timeout ) {
|
|
logicConditionStates[lcIndex].flags |= LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
|
|
return true;
|
|
} else if (logicConditionStates[lcIndex].flags & LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED) {
|
|
return true;
|
|
}
|
|
} else {
|
|
logicConditionStates[lcIndex].timeout = 0;
|
|
logicConditionStates[lcIndex].flags &= ~LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
|
|
}
|
|
|
|
return false;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_TIMER:
|
|
if ((logicConditionStates[lcIndex].timeout == 0) || (millis() > logicConditionStates[lcIndex].timeout && !currentValue)) {
|
|
logicConditionStates[lcIndex].timeout = millis() + operandA;
|
|
return true;
|
|
} else if (millis() > logicConditionStates[lcIndex].timeout && currentValue) {
|
|
logicConditionStates[lcIndex].timeout = millis() + operandB;
|
|
return false;
|
|
}
|
|
return currentValue;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_DELTA:
|
|
{
|
|
int difference = logicConditionStates[lcIndex].lastValue - operandA;
|
|
logicConditionStates[lcIndex].lastValue = operandA;
|
|
return ABS(difference) >= operandB;
|
|
}
|
|
break;
|
|
|
|
case LOGIC_CONDITION_GVAR_SET:
|
|
gvSet(operandA, operandB);
|
|
return operandB;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_GVAR_INC:
|
|
temporaryValue = gvGet(operandA) + operandB;
|
|
gvSet(operandA, temporaryValue);
|
|
return temporaryValue;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_GVAR_DEC:
|
|
temporaryValue = gvGet(operandA) - operandB;
|
|
gvSet(operandA, temporaryValue);
|
|
return temporaryValue;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_ADD:
|
|
return constrain(operandA + operandB, INT32_MIN, INT32_MAX);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_SUB:
|
|
return constrain(operandA - operandB, INT32_MIN, INT32_MAX);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_MUL:
|
|
return constrain(operandA * operandB, INT32_MIN, INT32_MAX);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_DIV:
|
|
if (operandB != 0) {
|
|
return constrain(operandA / operandB, INT32_MIN, INT32_MAX);
|
|
} else {
|
|
return operandA;
|
|
}
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY:
|
|
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY);
|
|
return true;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE:
|
|
logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE] = operandA;
|
|
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE);
|
|
return true;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_SWAP_ROLL_YAW:
|
|
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW);
|
|
return true;
|
|
break;
|
|
|
|
#ifdef USE_MAG
|
|
case LOGIC_CONDITION_RESET_MAG_CALIBRATION:
|
|
|
|
ENABLE_STATE(CALIBRATE_MAG);
|
|
return true;
|
|
break;
|
|
#endif
|
|
case LOGIC_CONDITION_SET_VTX_POWER_LEVEL:
|
|
#if defined(USE_VTX_CONTROL)
|
|
#if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP))
|
|
if (
|
|
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);
|
|
vtxSettingsConfigMutable()->power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL];
|
|
return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL];
|
|
} else {
|
|
return false;
|
|
}
|
|
break;
|
|
#else
|
|
return false;
|
|
#endif
|
|
|
|
case LOGIC_CONDITION_SET_VTX_BAND:
|
|
if (
|
|
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] != operandA &&
|
|
vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
|
|
) {
|
|
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = constrain(operandA, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND);
|
|
vtxSettingsConfigMutable()->band = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND];
|
|
return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND];
|
|
} else {
|
|
return false;
|
|
}
|
|
break;
|
|
case LOGIC_CONDITION_SET_VTX_CHANNEL:
|
|
if (
|
|
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] != operandA &&
|
|
vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
|
|
) {
|
|
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL);
|
|
vtxSettingsConfigMutable()->channel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL];
|
|
return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL];
|
|
} else {
|
|
return false;
|
|
}
|
|
break;
|
|
#endif
|
|
case LOGIC_CONDITION_INVERT_ROLL:
|
|
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL);
|
|
return true;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_INVERT_PITCH:
|
|
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);
|
|
return operandA;
|
|
break;
|
|
|
|
#ifdef USE_I2C_IO_EXPANDER
|
|
case LOGIC_CONDITION_PORT_SET:
|
|
ioPortExpanderSet((uint8_t)operandA, (uint8_t)operandB);
|
|
return operandB;
|
|
break;
|
|
#endif
|
|
|
|
case LOGIC_CONDITION_SIN:
|
|
temporaryValue = (operandB == 0) ? 500 : operandB;
|
|
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;
|
|
break;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_TAN:
|
|
temporaryValue = (operandB == 0) ? 500 : operandB;
|
|
return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_MIN:
|
|
return (operandA < operandB) ? operandA : operandB;
|
|
break;
|
|
|
|
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;
|
|
|
|
case LOGIC_CONDITION_RC_CHANNEL_OVERRIDE:
|
|
temporaryValue = constrain(operandA - 1, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT - 1);
|
|
rcChannelOverrides[temporaryValue].active = true;
|
|
rcChannelOverrides[temporaryValue].value = constrain(operandB, PWM_RANGE_MIN, PWM_RANGE_MAX);
|
|
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL);
|
|
return true;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_SET_HEADING_TARGET:
|
|
temporaryValue = CENTIDEGREES_TO_DEGREES(wrap_36000(DEGREES_TO_CENTIDEGREES(operandA)));
|
|
updateHeadingHoldTarget(temporaryValue);
|
|
return temporaryValue;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_MODULUS:
|
|
if (operandB != 0) {
|
|
return constrain(operandA % operandB, INT32_MIN, INT32_MAX);
|
|
} else {
|
|
return operandA;
|
|
}
|
|
break;
|
|
|
|
case LOGIC_CONDITION_SET_PROFILE:
|
|
operandA--;
|
|
if ( getConfigProfile() != operandA && (operandA >= 0 && operandA < MAX_PROFILE_COUNT)) {
|
|
bool profileChanged = false;
|
|
if (setConfigProfile(operandA)) {
|
|
pidInit();
|
|
pidInitFilters();
|
|
schedulePidGainsUpdate();
|
|
navigationUsePIDs(); //set navigation pid gains
|
|
profileChanged = true;
|
|
}
|
|
return profileChanged;
|
|
} else {
|
|
return false;
|
|
}
|
|
break;
|
|
|
|
case LOGIC_CONDITION_LOITER_OVERRIDE:
|
|
logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE] = constrain(operandA, 0, 100000);
|
|
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS);
|
|
return true;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE:
|
|
if (operandA >= 0 && operandA <= 2) {
|
|
|
|
flightAxisOverride[operandA].angleTargetActive = true;
|
|
int target = DEGREES_TO_DECIDEGREES(operandB);
|
|
if (operandA == 0) {
|
|
//ROLL
|
|
target = constrain(target, -pidProfile()->max_angle_inclination[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
|
|
} else if (operandA == 1) {
|
|
//PITCH
|
|
target = constrain(target, -pidProfile()->max_angle_inclination[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]);
|
|
} else if (operandA == 2) {
|
|
//YAW
|
|
target = (constrain(target, 0, 3600));
|
|
}
|
|
flightAxisOverride[operandA].angleTarget = target;
|
|
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS);
|
|
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
break;
|
|
|
|
case LOGIC_CONDITION_FLIGHT_AXIS_RATE_OVERRIDE:
|
|
if (operandA >= 0 && operandA <= 2) {
|
|
flightAxisOverride[operandA].rateTargetActive = true;
|
|
flightAxisOverride[operandA].rateTarget = constrain(operandB, -2000, 2000);
|
|
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS);
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
break;
|
|
|
|
#ifdef USE_LED_STRIP
|
|
case LOGIC_CONDITION_LED_PIN_PWM:
|
|
|
|
if (operandA >=0 && operandA <= 100) {
|
|
ledPinStartPWM((uint8_t)operandA);
|
|
} else {
|
|
ledPinStopPWM();
|
|
}
|
|
return operandA;
|
|
break;
|
|
#endif
|
|
#ifdef USE_GPS_FIX_ESTIMATION
|
|
case LOGIC_CONDITION_DISABLE_GPS_FIX:
|
|
if (operandA > 0) {
|
|
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
|
|
} else {
|
|
LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
|
|
}
|
|
return true;
|
|
break;
|
|
#endif
|
|
|
|
default:
|
|
return false;
|
|
break;
|
|
}
|
|
}
|
|
|
|
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
|
|
*/
|
|
if (!(logicConditionStates[i].flags & LOGIC_CONDITION_FLAG_LATCH)) {
|
|
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,
|
|
operandBValue,
|
|
i
|
|
);
|
|
|
|
logicConditionStates[i].value = newValue;
|
|
|
|
/*
|
|
* if value evaluates as true, put a latch on logic condition
|
|
*/
|
|
if (logicConditions(i)->flags & LOGIC_CONDITION_FLAG_LATCH && newValue) {
|
|
logicConditionStates[i].flags |= LOGIC_CONDITION_FLAG_LATCH;
|
|
}
|
|
}
|
|
} else {
|
|
logicConditionStates[i].value = false;
|
|
}
|
|
}
|
|
|
|
static int logicConditionGetWaypointOperandValue(int operand) {
|
|
|
|
switch (operand) {
|
|
case LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP: // 0/1
|
|
return (navGetCurrentStateFlags() & NAV_AUTO_WP) ? 1 : 0;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX:
|
|
return NAV_Status.activeWpNumber;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION:
|
|
return NAV_Status.activeWpAction;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_WAYPOINTS_NEXT_WAYPOINT_ACTION:
|
|
{
|
|
uint8_t wpIndex = posControl.activeWaypointIndex + 1;
|
|
if ((wpIndex > 0) && (wpIndex < NAV_MAX_WAYPOINTS)) {
|
|
return posControl.waypointList[wpIndex].action;
|
|
}
|
|
return false;
|
|
}
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE:
|
|
{
|
|
uint32_t distance = 0;
|
|
if (navGetCurrentStateFlags() & NAV_AUTO_WP) {
|
|
fpVector3_t poi;
|
|
gpsLocation_t wp;
|
|
wp.lat = posControl.waypointList[NAV_Status.activeWpIndex].lat;
|
|
wp.lon = posControl.waypointList[NAV_Status.activeWpIndex].lon;
|
|
wp.alt = posControl.waypointList[NAV_Status.activeWpIndex].alt;
|
|
geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE);
|
|
|
|
distance = calculateDistanceToDestination(&poi) / 100;
|
|
}
|
|
|
|
return distance;
|
|
}
|
|
break;
|
|
|
|
case LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT:
|
|
{
|
|
uint32_t distance = 0;
|
|
if ((navGetCurrentStateFlags() & NAV_AUTO_WP) && NAV_Status.activeWpIndex > 0) {
|
|
fpVector3_t poi;
|
|
gpsLocation_t wp;
|
|
wp.lat = posControl.waypointList[NAV_Status.activeWpIndex-1].lat;
|
|
wp.lon = posControl.waypointList[NAV_Status.activeWpIndex-1].lon;
|
|
wp.alt = posControl.waypointList[NAV_Status.activeWpIndex-1].alt;
|
|
geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE);
|
|
|
|
distance = calculateDistanceToDestination(&poi) / 100;
|
|
}
|
|
|
|
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;
|
|
|
|
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION:
|
|
return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER2) == NAV_WP_USER2) : 0;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION:
|
|
return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER3) == NAV_WP_USER3) : 0;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION:
|
|
return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER4) == NAV_WP_USER4) : 0;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION_NEXT_WP:
|
|
return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER1) == NAV_WP_USER1);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION_NEXT_WP:
|
|
return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER2) == NAV_WP_USER2);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION_NEXT_WP:
|
|
return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER3) == NAV_WP_USER3);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION_NEXT_WP:
|
|
return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER4) == NAV_WP_USER4);
|
|
break;
|
|
|
|
default:
|
|
return 0;
|
|
break;
|
|
}
|
|
}
|
|
|
|
static int logicConditionGetFlightOperandValue(int operand) {
|
|
|
|
switch (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;
|
|
|
|
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;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE: // V / 10
|
|
return getBatteryAverageCellVoltage();
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS:
|
|
return getBatteryCellCount();
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100
|
|
return getAmperage();
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh
|
|
return getMAhDrawn();
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS:
|
|
#ifdef USE_GPS_FIX_ESTIMATION
|
|
if ( STATE(GPS_ESTIMATED_FIX) ){
|
|
return gpsSol.numSat; //99
|
|
} else
|
|
#endif
|
|
if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
|
|
return 0;
|
|
} else {
|
|
return gpsSol.numSat;
|
|
}
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1
|
|
return STATE(GPS_FIX) ? 1 : 0;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED: // cm/s
|
|
return gpsSol.groundSpeed;
|
|
break;
|
|
|
|
//FIXME align with osdGet3DSpeed
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s
|
|
return osdGet3DSpeed();
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s
|
|
#ifdef USE_PITOT
|
|
return constrain(getAirspeedEstimate(), 0, INT16_MAX);
|
|
#else
|
|
return false;
|
|
#endif
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE: // cm
|
|
return constrain(getEstimatedActualPosition(Z), INT16_MIN, INT16_MAX);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s
|
|
return constrain(getEstimatedActualVelocity(Z), INT16_MIN, INT16_MAX);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // %
|
|
return (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL: // deg
|
|
return constrain(attitude.values.roll / 10, -180, 180);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH: // deg
|
|
return constrain(attitude.values.pitch / 10, -180, 180);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW: // deg
|
|
return constrain(attitude.values.yaw / 10, 0, 360);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED: // 0/1
|
|
return ARMING_FLAG(ARMED) ? 1 : 0;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1
|
|
return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1
|
|
return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1
|
|
return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1
|
|
return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1
|
|
return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
|
|
#ifdef USE_FW_AUTOLAND
|
|
return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || FLIGHT_MODE(NAV_FW_AUTOLAND)) ? 1 : 0;
|
|
#else
|
|
return ((navGetCurrentStateFlags() & NAV_CTL_LAND)) ? 1 : 0;
|
|
#endif
|
|
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1
|
|
return (failsafePhase() != FAILSAFE_IDLE) ? 1 : 0;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: //
|
|
return axisPID[YAW];
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: //
|
|
return axisPID[ROLL];
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: //
|
|
return axisPID[PITCH];
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m
|
|
return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT16_MAX);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ:
|
|
#ifdef USE_SERIALRX_CRSF
|
|
return rxLinkStatistics.uplinkLQ;
|
|
#else
|
|
return 0;
|
|
#endif
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR:
|
|
#ifdef USE_SERIALRX_CRSF
|
|
return rxLinkStatistics.uplinkSNR;
|
|
#else
|
|
return 0;
|
|
#endif
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int
|
|
return getConfigProfile() + 1;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE: //int
|
|
return getConfigBatteryProfile() + 1;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int
|
|
return currentMixerProfileIndex + 1;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE: //0,1
|
|
return isMixerTransitionMixing ? 1 : 0;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS:
|
|
return getLoiterRadius(navConfig()->fw.loiter_radius);
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS:
|
|
return isEstimatedAglTrusted();
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_AGL:
|
|
return getEstimatedAglPosition();
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW:
|
|
return rangefinderGetLatestRawAltitude();
|
|
break;
|
|
#ifdef USE_FW_AUTOLAND
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE:
|
|
return posControl.fwLandState.landState;
|
|
break;
|
|
#endif
|
|
default:
|
|
return 0;
|
|
break;
|
|
}
|
|
}
|
|
|
|
static int logicConditionGetFlightModeOperandValue(int operand) {
|
|
|
|
switch (operand) {
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_FAILSAFE:
|
|
return (bool) FLIGHT_MODE(FAILSAFE_MODE);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_MANUAL:
|
|
return (bool) FLIGHT_MODE(MANUAL_MODE);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_RTH:
|
|
return (bool) FLIGHT_MODE(NAV_RTH_MODE);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_POSHOLD:
|
|
return (bool) FLIGHT_MODE(NAV_POSHOLD_MODE);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION:
|
|
return (bool) FLIGHT_MODE(NAV_WP_MODE);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD:
|
|
return ((bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !(bool)FLIGHT_MODE(NAV_ALTHOLD_MODE));
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE:
|
|
return (bool) (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE));
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD:
|
|
return (bool) FLIGHT_MODE(NAV_ALTHOLD_MODE);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLE:
|
|
return (bool) FLIGHT_MODE(ANGLE_MODE);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_HORIZON:
|
|
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(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:
|
|
return IS_RC_MODE_ACTIVE(BOXUSER1);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2:
|
|
return IS_RC_MODE_ACTIVE(BOXUSER2);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER3:
|
|
return IS_RC_MODE_ACTIVE(BOXUSER3);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4:
|
|
return IS_RC_MODE_ACTIVE(BOXUSER4);
|
|
break;
|
|
|
|
default:
|
|
return 0;
|
|
break;
|
|
}
|
|
}
|
|
|
|
int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
|
|
int retVal = 0;
|
|
|
|
switch (type) {
|
|
|
|
case LOGIC_CONDITION_OPERAND_TYPE_VALUE:
|
|
retVal = operand;
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL:
|
|
//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:
|
|
retVal = logicConditionGetFlightOperandValue(operand);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT_MODE:
|
|
retVal = logicConditionGetFlightModeOperandValue(operand);
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_TYPE_LC:
|
|
if (operand >= 0 && operand < MAX_LOGIC_CONDITIONS) {
|
|
retVal = logicConditionGetValue(operand);
|
|
}
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_TYPE_GVAR:
|
|
if (operand >= 0 && operand < MAX_GLOBAL_VARIABLES) {
|
|
retVal = gvGet(operand);
|
|
}
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_TYPE_PID:
|
|
if (operand >= 0 && operand < MAX_PROGRAMMING_PID_COUNT) {
|
|
retVal = programmingPidGetOutput(operand);
|
|
}
|
|
break;
|
|
|
|
case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS:
|
|
retVal = logicConditionGetWaypointOperandValue(operand);
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return retVal;
|
|
}
|
|
|
|
/*
|
|
* conditionId == -1 is always evaluated as true
|
|
*/
|
|
int logicConditionGetValue(int8_t conditionId) {
|
|
if (conditionId >= 0) {
|
|
return logicConditionStates[conditionId].value;
|
|
} else {
|
|
return true;
|
|
}
|
|
}
|
|
|
|
void logicConditionUpdateTask(timeUs_t currentTimeUs) {
|
|
UNUSED(currentTimeUs);
|
|
|
|
//Disable all flags
|
|
logicConditionsGlobalFlags = 0;
|
|
|
|
for (uint8_t i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
|
|
rcChannelOverrides[i].active = false;
|
|
}
|
|
|
|
for (uint8_t i = 0; i < XYZ_AXIS_COUNT; i++) {
|
|
flightAxisOverride[i].rateTargetActive = false;
|
|
flightAxisOverride[i].angleTargetActive = false;
|
|
}
|
|
|
|
for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
|
|
logicConditionProcess(i);
|
|
}
|
|
|
|
#ifdef USE_I2C_IO_EXPANDER
|
|
ioPortExpanderSync();
|
|
#endif
|
|
}
|
|
|
|
void logicConditionReset(void) {
|
|
for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
|
|
logicConditionStates[i].value = 0;
|
|
logicConditionStates[i].lastValue = 0;
|
|
logicConditionStates[i].flags = 0;
|
|
logicConditionStates[i].timeout = 0;
|
|
}
|
|
}
|
|
|
|
float NOINLINE getThrottleScale(float globalThrottleScale) {
|
|
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE)) {
|
|
return constrainf(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE] / 100.0f, 0.0f, 1.0f);
|
|
} else {
|
|
return globalThrottleScale;
|
|
}
|
|
}
|
|
|
|
int16_t getRcCommandOverride(int16_t command[], uint8_t axis) {
|
|
int16_t outputValue = command[axis];
|
|
|
|
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_ROLL) {
|
|
outputValue = command[FD_YAW];
|
|
} else if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_YAW) {
|
|
outputValue = command[FD_ROLL];
|
|
}
|
|
|
|
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL) && axis == FD_ROLL) {
|
|
outputValue *= -1;
|
|
}
|
|
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH) && axis == FD_PITCH) {
|
|
outputValue *= -1;
|
|
}
|
|
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW) && axis == FD_YAW) {
|
|
outputValue *= -1;
|
|
}
|
|
|
|
return outputValue;
|
|
}
|
|
|
|
int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) {
|
|
if (rcChannelOverrides[channel].active) {
|
|
return rcChannelOverrides[channel].value;
|
|
} else {
|
|
return originalValue;
|
|
}
|
|
}
|
|
|
|
uint32_t getLoiterRadius(uint32_t loiterRadius) {
|
|
#ifdef USE_PROGRAMMING_FRAMEWORK
|
|
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 {
|
|
return loiterRadius;
|
|
}
|
|
#else
|
|
return loiterRadius;
|
|
#endif
|
|
}
|
|
|
|
float getFlightAxisAngleOverride(uint8_t axis, float angle) {
|
|
if (flightAxisOverride[axis].angleTargetActive) {
|
|
return flightAxisOverride[axis].angleTarget;
|
|
} else {
|
|
return angle;
|
|
}
|
|
}
|
|
|
|
float getFlightAxisRateOverride(uint8_t axis, float rate) {
|
|
if (flightAxisOverride[axis].rateTargetActive) {
|
|
return flightAxisOverride[axis].rateTarget;
|
|
} else {
|
|
return rate;
|
|
}
|
|
}
|
|
|
|
bool isFlightAxisAngleOverrideActive(uint8_t axis) {
|
|
if (flightAxisOverride[axis].angleTargetActive) {
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool isFlightAxisRateOverrideActive(uint8_t axis) {
|
|
if (flightAxisOverride[axis].rateTargetActive) {
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|