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

Merge remote-tracking branch 'origin/master' into sh_mixer_profile

Browse source
This commit is contained in:
shota 2023-06-26 02:33:09 +09:00
commit 3ad0991125
602 changed files with 160748 additions and 4958 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -87,13 +87,16 @@ void pgResetFn_logicConditions(logicCondition_t *instance)
logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
static int logicConditionCompute(
int32_t currentVaue,
int32_t currentValue,
logicOperation_e operation,
int32_t operandA,
int32_t operandB
int32_t operandB,
uint8_t lcIndex
) {
int temporaryValue;
#if defined(USE_VTX_CONTROL)
vtxDeviceCapability_t vtxDeviceCapability;
#endif
switch (operation) {
@ -105,6 +108,13 @@ static int logicConditionCompute(
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;
@ -160,7 +170,67 @@ static int logicConditionCompute(
}
//When both operands are not met, keep current value
return currentVaue;
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:
@ -217,7 +287,8 @@ static int logicConditionCompute(
break;
case LOGIC_CONDITION_SET_VTX_POWER_LEVEL:
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
#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)
@ -257,7 +328,7 @@ static int logicConditionCompute(
return false;
}
break;
#endif
case LOGIC_CONDITION_INVERT_ROLL:
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL);
return true;
@ -437,7 +508,8 @@ void logicConditionProcess(uint8_t i) {
logicConditionStates[i].value,
logicConditions(i)->operation,
operandAValue,
operandBValue
operandBValue,
i
);
logicConditionStates[i].value = newValue;
@ -454,6 +526,105 @@ void logicConditionProcess(uint8_t i) {
}
}
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) {
@ -567,10 +738,6 @@ static int logicConditionGetFlightOperandValue(int operand) {
return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_WP: // 0/1
return (navGetCurrentStateFlags() & NAV_AUTO_WP) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0;
break;
@ -591,14 +758,6 @@ static int logicConditionGetFlightOperandValue(int operand) {
return axisPID[PITCH];
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_INDEX:
return NAV_Status.activeWpNumber;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_ACTION:
return NAV_Status.activeWpAction;
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;
@ -668,12 +827,16 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
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);
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));
return (bool) (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE));
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD:
@ -692,6 +855,11 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
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);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1:
return IS_RC_MODE_ACTIVE(BOXUSER1);
break;
@ -704,6 +872,10 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
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;
@ -721,7 +893,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
case LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL:
//Extract RC channel raw value
if (operand >= 1 && operand <= 16) {
if (operand >= 1 && operand <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
retVal = rxGetChannelValue(operand - 1);
}
break;
@ -751,6 +923,10 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
retVal = programmingPidGetOutput(operand);
}
break;
case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS:
retVal = logicConditionGetWaypointOperandValue(operand);
break;
default:
break;
@ -797,7 +973,9 @@ void logicConditionUpdateTask(timeUs_t currentTimeUs) {
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;
}
}