Skip to content

Commit

Permalink
add flown loiter radius for fixed wing
Browse files Browse the repository at this point in the history
  • Loading branch information
@breadoven
breadoven committed Jun 29, 2024
1 parent 41c89a7 commit 817981f
Show file tree
Hide file tree
Showing 4 changed files with 65 additions and 51 deletions.
9 changes: 8 additions & 1 deletion src/main/navigation/navigation.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5074,5 +5074,12 @@ bool canFwLandingBeCancelled(void)
{
return FLIGHT_MODE(NAV_FW_AUTOLAND) && posControl.navState != NAV_STATE_FW_LANDING_FLARE;
}

#endif
uint16_t getFlownLoiterRadius(void)
{
if (STATE(AIRPLANE) && navGetCurrentStateFlags() & NAV_CTL_HOLD) {
return CENTIMETERS_TO_METERS(calculateDistanceToDestination(&posControl.desiredState.pos));
}

return 0;
}
1 change: 1 addition & 0 deletions src/main/navigation/navigation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -694,6 +694,7 @@ bool rthAltControlStickOverrideCheck(uint8_t axis);

int8_t navCheckActiveAngleHoldAxis(void);
uint8_t getActiveWpNumber(void);
uint16_t getFlownLoiterRadius(void);

/* Returns the heading recorded when home position was acquired.
* Note that the navigation system uses deg*100 as unit and angles
Expand Down
105 changes: 55 additions & 50 deletions src/main/programming/logic_condition.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,7 @@ static int logicConditionCompute(
int temporaryValue;
#if defined(USE_VTX_CONTROL)
vtxDeviceCapability_t vtxDeviceCapability;
#endif
#endif

switch (operation) {

Expand Down Expand Up @@ -154,7 +154,7 @@ static int logicConditionCompute(

case LOGIC_CONDITION_NOR:
return !(operandA || operandB);
break;
break;

case LOGIC_CONDITION_NOT:
return !operandA;
Expand All @@ -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;

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand All @@ -293,12 +293,12 @@ static int logicConditionCompute(
ENABLE_STATE(CALIBRATE_MAG);
return true;
break;
#endif
#endif
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);
Expand Down Expand Up @@ -346,18 +346,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);
Expand All @@ -373,18 +373,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:
Expand All @@ -394,11 +394,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;
Expand Down Expand Up @@ -507,7 +507,7 @@ static int logicConditionCompute(

default:
return false;
break;
break;
}
}

Expand All @@ -516,7 +516,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
Expand All @@ -525,13 +525,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;

/*
Expand Down Expand Up @@ -606,7 +606,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;
Expand Down Expand Up @@ -652,19 +652,19 @@ 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;

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;
Expand All @@ -680,7 +680,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;
Expand All @@ -697,7 +697,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;
Expand Down Expand Up @@ -749,46 +749,46 @@ 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;
break;

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
#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: //

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;

Expand Down Expand Up @@ -819,7 +819,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
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;
Expand All @@ -830,15 +830,20 @@ static int logicConditionGetFlightOperandValue(int operand) {

case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS:
return getLoiterRadius(navConfig()->fw.loiter_radius);
break;

case LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS:
return getFlownLoiterRadius();
break;

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;
Expand Down Expand Up @@ -946,7 +951,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:
Expand Down Expand Up @@ -974,7 +979,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
retVal = programmingPidGetOutput(operand);
}
break;

case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS:
retVal = logicConditionGetWaypointOperandValue(operand);
break;
Expand All @@ -988,7 +993,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;
Expand Down Expand Up @@ -1070,7 +1075,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 {
Expand Down
1 change: 1 addition & 0 deletions src/main/programming/logic_condition.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -143,6 +143,7 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW, // deg // 40
LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE, // 41
LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE, // int // 42
LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS, // 43
} logicFlightOperands_e;

typedef enum {
Expand Down

0 comments on commit 817981f

Please sign in to comment.