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

fixes + telemetry

Browse source
This commit is contained in:
breadoven 2023-09-14 15:20:43 +01:00
parent 0f7ecda8b7
commit 221d8fe3ad
8 changed files with 70 additions and 60 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/fc/runtime_config.c
View file

@ -173,6 +173,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
if (FLIGHT_MODE(HORIZON_MODE)) if (FLIGHT_MODE(HORIZON_MODE))
return FLM_HORIZON; return FLM_HORIZON;
if (FLIGHT_MODE(ATTIHOLD_MODE))
return FLM_ATTIHOLD;
return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO; return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO;
} }

1
src/main/fc/runtime_config.h
View file

@ -162,6 +162,7 @@ typedef enum {
FLM_CRUISE, FLM_CRUISE,
FLM_LAUNCH, FLM_LAUNCH,
FLM_FAILSAFE, FLM_FAILSAFE,
FLM_ATTIHOLD,
FLM_COUNT FLM_COUNT
} flightModeForTelemetry_e; } flightModeForTelemetry_e;

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

@ -1110,8 +1110,11 @@ void FAST_CODE pidController(float dT)
// Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ATTI_MODE // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ATTI_MODE
const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f; const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f;
levelingEnabled = false; levelingEnabled = false;
static bool restartAttiMode = true; static bool restartAttiMode = true;
bool attiModeActive = false; if (!restartAttiMode) {
restartAttiMode = !FLIGHT_MODE(ATTIHOLD_MODE); // set restart flag if attihold_mode not active
}
for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) { for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ATTIHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) { if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ATTIHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) {
@ -1120,7 +1123,6 @@ void FAST_CODE pidController(float dT)
if (FLIGHT_MODE(ATTIHOLD_MODE) && !isFlightAxisAngleOverrideActive(axis)) { if (FLIGHT_MODE(ATTIHOLD_MODE) && !isFlightAxisAngleOverrideActive(axis)) {
static int16_t attiHoldTarget[2]; static int16_t attiHoldTarget[2];
attiModeActive = true;
if (restartAttiMode) { // set target attitude to current attitude when initialised if (restartAttiMode) { // set target attitude to current attitude when initialised
attiHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL]; attiHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
@ -1143,9 +1145,6 @@ void FAST_CODE pidController(float dT)
levelingEnabled = true; levelingEnabled = true;
} }
} }
if (!restartAttiMode) { // set restart flag if attihold_mode not active on previous loop
restartAttiMode = !attiModeActive;
}
// Apply Turn Assistance // Apply Turn Assistance
if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {

1
src/main/io/osd_dji_hd.c
View file

@ -300,6 +300,7 @@ static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
case FLM_ALTITUDE_HOLD: case FLM_ALTITUDE_HOLD:
case FLM_POSITION_HOLD: case FLM_POSITION_HOLD:
case FLM_MISSION: case FLM_MISSION:
case FLM_ATTIHOLD:
default: default:
// Unsupported ATM, keep at ANGLE // Unsupported ATM, keep at ANGLE
bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE

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

@ -95,7 +95,7 @@ static int logicConditionCompute(
int temporaryValue; int temporaryValue;
#if defined(USE_VTX_CONTROL) #if defined(USE_VTX_CONTROL)
vtxDeviceCapability_t vtxDeviceCapability; vtxDeviceCapability_t vtxDeviceCapability;
#endif #endif
switch (operation) { switch (operation) {
@ -152,7 +152,7 @@ static int logicConditionCompute(
case LOGIC_CONDITION_NOR: case LOGIC_CONDITION_NOR:
return !(operandA || operandB); return !(operandA || operandB);
break; break;
case LOGIC_CONDITION_NOT: case LOGIC_CONDITION_NOT:
return !operandA; return !operandA;
@ -168,7 +168,7 @@ static int logicConditionCompute(
return false; return false;
} }
//When both operands are not met, keep current value //When both operands are not met, keep current value
return currentValue; return currentValue;
break; break;
@ -236,7 +236,7 @@ static int logicConditionCompute(
gvSet(operandA, operandB); gvSet(operandA, operandB);
return operandB; return operandB;
break; break;
case LOGIC_CONDITION_GVAR_INC: case LOGIC_CONDITION_GVAR_INC:
temporaryValue = gvGet(operandA) + operandB; temporaryValue = gvGet(operandA) + operandB;
gvSet(operandA, temporaryValue); gvSet(operandA, temporaryValue);
@ -268,7 +268,7 @@ static int logicConditionCompute(
return operandA; return operandA;
} }
break; break;
case LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY: case LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY:
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY); LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY);
return true; return true;
@ -286,10 +286,10 @@ static int logicConditionCompute(
break; break;
case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: 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(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP))
if ( if (
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA && logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA &&
vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability) vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
) { ) {
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
@ -337,18 +337,18 @@ static int logicConditionCompute(
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH); LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH);
return true; return true;
break; break;
case LOGIC_CONDITION_INVERT_YAW: case LOGIC_CONDITION_INVERT_YAW:
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW); LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW);
return true; return true;
break; break;
case LOGIC_CONDITION_OVERRIDE_THROTTLE: case LOGIC_CONDITION_OVERRIDE_THROTTLE:
logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE] = operandA; logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE] = operandA;
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE); LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE);
return operandA; return operandA;
break; break;
case LOGIC_CONDITION_SET_OSD_LAYOUT: case LOGIC_CONDITION_SET_OSD_LAYOUT:
logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT] = operandA; logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT] = operandA;
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT); LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT);
@ -364,18 +364,18 @@ static int logicConditionCompute(
case LOGIC_CONDITION_SIN: case LOGIC_CONDITION_SIN:
temporaryValue = (operandB == 0) ? 500 : operandB; temporaryValue = (operandB == 0) ? 500 : operandB;
return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
break; break;
case LOGIC_CONDITION_COS: case LOGIC_CONDITION_COS:
temporaryValue = (operandB == 0) ? 500 : operandB; temporaryValue = (operandB == 0) ? 500 : operandB;
return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
break; break;
break; break;
case LOGIC_CONDITION_TAN: case LOGIC_CONDITION_TAN:
temporaryValue = (operandB == 0) ? 500 : operandB; temporaryValue = (operandB == 0) ? 500 : operandB;
return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
break; break;
case LOGIC_CONDITION_MIN: case LOGIC_CONDITION_MIN:
@ -385,11 +385,11 @@ static int logicConditionCompute(
case LOGIC_CONDITION_MAX: case LOGIC_CONDITION_MAX:
return (operandA > operandB) ? operandA : operandB; return (operandA > operandB) ? operandA : operandB;
break; break;
case LOGIC_CONDITION_MAP_INPUT: case LOGIC_CONDITION_MAP_INPUT:
return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000); return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000);
break; break;
case LOGIC_CONDITION_MAP_OUTPUT: case LOGIC_CONDITION_MAP_OUTPUT:
return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB); return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB);
break; break;
@ -471,11 +471,11 @@ static int logicConditionCompute(
} else { } else {
return false; return false;
} }
break; break;
default: default:
return false; return false;
break; break;
} }
} }
@ -484,7 +484,7 @@ void logicConditionProcess(uint8_t i) {
const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId); const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId);
if (logicConditions(i)->enabled && activatorValue && !cliMode) { if (logicConditions(i)->enabled && activatorValue && !cliMode) {
/* /*
* Process condition only when latch flag is not set * Process condition only when latch flag is not set
* Latched LCs can only go from OFF to ON, not the other way * Latched LCs can only go from OFF to ON, not the other way
@ -493,13 +493,13 @@ void logicConditionProcess(uint8_t i) {
const int operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value); 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 operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value);
const int newValue = logicConditionCompute( const int newValue = logicConditionCompute(
logicConditionStates[i].value, logicConditionStates[i].value,
logicConditions(i)->operation, logicConditions(i)->operation,
operandAValue, operandAValue,
operandBValue, operandBValue,
i i
); );
logicConditionStates[i].value = newValue; logicConditionStates[i].value = newValue;
/* /*
@ -574,7 +574,7 @@ static int logicConditionGetWaypointOperandValue(int operand) {
return distance; return distance;
} }
break; break;
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION: 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; return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0;
break; break;
@ -620,11 +620,11 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s
return constrain((uint32_t)getFlightTime(), 0, INT16_MAX); return constrain((uint32_t)getFlightTime(), 0, INT16_MAX);
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m
return constrain(GPS_distanceToHome, 0, INT16_MAX); return constrain(GPS_distanceToHome, 0, INT16_MAX);
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m
return constrain(getTotalTravelDistance() / 100, 0, INT16_MAX); return constrain(getTotalTravelDistance() / 100, 0, INT16_MAX);
break; break;
@ -632,7 +632,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI: case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI:
return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100 case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100
return getBatteryVoltage(); return getBatteryVoltage();
break; break;
@ -648,7 +648,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100 case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100
return getAmperage(); return getAmperage();
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh
return getMAhDrawn(); return getMAhDrawn();
break; break;
@ -660,7 +660,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
return gpsSol.numSat; return gpsSol.numSat;
} }
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1 case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1
return STATE(GPS_FIX) ? 1 : 0; return STATE(GPS_FIX) ? 1 : 0;
break; break;
@ -708,15 +708,15 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1 case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0; return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0;
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1 case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0; return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0;
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1 case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0; return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0;
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1 case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0; return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0;
@ -724,7 +724,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1 case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1
return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0; return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0;
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1 case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0; return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0;
@ -733,16 +733,16 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1 case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1
return (failsafePhase() != FAILSAFE_IDLE) ? 1 : 0; return (failsafePhase() != FAILSAFE_IDLE) ? 1 : 0;
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: // case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: //
return axisPID[YAW]; return axisPID[YAW];
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: // case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: //
return axisPID[ROLL]; return axisPID[ROLL];
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: // case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: //
return axisPID[PITCH]; return axisPID[PITCH];
break; break;
@ -776,14 +776,14 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS: case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS:
return isEstimatedAglTrusted(); return isEstimatedAglTrusted();
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_AGL: case LOGIC_CONDITION_OPERAND_FLIGHT_AGL:
return getEstimatedAglPosition(); return getEstimatedAglPosition();
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW: case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW:
return rangefinderGetLatestRawAltitude(); return rangefinderGetLatestRawAltitude();
break; break;
default: default:
return 0; return 0;
@ -835,12 +835,16 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
return (bool) FLIGHT_MODE(HORIZON_MODE); return (bool) FLIGHT_MODE(HORIZON_MODE);
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ATTIMODE:
return (bool) FLIGHT_MODE(ATTIHOLD_MODE);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR: case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR:
return (bool) FLIGHT_MODE(AIRMODE_ACTIVE); return (bool) FLIGHT_MODE(AIRMODE_ACTIVE);
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO: 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) || 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); (bool) FLIGHT_MODE(NAV_POSHOLD_MODE) || (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (bool) FLIGHT_MODE(NAV_WP_MODE)) == false);
break; break;
@ -879,7 +883,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
//Extract RC channel raw value //Extract RC channel raw value
if (operand >= 1 && operand <= MAX_SUPPORTED_RC_CHANNEL_COUNT) { if (operand >= 1 && operand <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
retVal = rxGetChannelValue(operand - 1); retVal = rxGetChannelValue(operand - 1);
} }
break; break;
case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT: case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT:
@ -907,7 +911,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
retVal = programmingPidGetOutput(operand); retVal = programmingPidGetOutput(operand);
} }
break; break;
case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS: case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS:
retVal = logicConditionGetWaypointOperandValue(operand); retVal = logicConditionGetWaypointOperandValue(operand);
break; break;
@ -921,7 +925,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
/* /*
* conditionId == -1 is always evaluated as true * conditionId == -1 is always evaluated as true
*/ */
int logicConditionGetValue(int8_t conditionId) { int logicConditionGetValue(int8_t conditionId) {
if (conditionId >= 0) { if (conditionId >= 0) {
return logicConditionStates[conditionId].value; return logicConditionStates[conditionId].value;
@ -1003,7 +1007,7 @@ int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) {
uint32_t getLoiterRadius(uint32_t loiterRadius) { uint32_t getLoiterRadius(uint32_t loiterRadius) {
#ifdef USE_PROGRAMMING_FRAMEWORK #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())) { !(FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding())) {
return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000); return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000);
} else { } else {

1
src/main/programming/logic_condition.h
View file

@ -154,6 +154,7 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4, // 13 LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4, // 13
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO, // 14 LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO, // 14
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION, // 15 LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION, // 15
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ATTIMODE, // 16
} logicFlightModeOperands_e; } logicFlightModeOperands_e;
typedef enum { typedef enum {

8
src/main/telemetry/mavlink.c
View file

@ -191,6 +191,7 @@ static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode)
case FLM_ACRO_AIR: return COPTER_MODE_ACRO; case FLM_ACRO_AIR: return COPTER_MODE_ACRO;
case FLM_ANGLE: return COPTER_MODE_STABILIZE; case FLM_ANGLE: return COPTER_MODE_STABILIZE;
case FLM_HORIZON: return COPTER_MODE_STABILIZE; case FLM_HORIZON: return COPTER_MODE_STABILIZE;
case FLM_ATTIHOLD: return COPTER_MODE_STABILIZE;
case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD; case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD;
case FLM_POSITION_HOLD: return COPTER_MODE_POSHOLD; case FLM_POSITION_HOLD: return COPTER_MODE_POSHOLD;
case FLM_RTH: return COPTER_MODE_RTL; 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_ACRO_AIR: return PLANE_MODE_ACRO;
case FLM_ANGLE: return PLANE_MODE_FLY_BY_WIRE_A; case FLM_ANGLE: return PLANE_MODE_FLY_BY_WIRE_A;
case FLM_HORIZON: return PLANE_MODE_STABILIZE; case FLM_HORIZON: return PLANE_MODE_STABILIZE;
case FLM_ATTIHOLD: return PLANE_MODE_STABILIZE;
case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B; case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B;
case FLM_POSITION_HOLD: return PLANE_MODE_LOITER; case FLM_POSITION_HOLD: return PLANE_MODE_LOITER;
case FLM_RTH: return PLANE_MODE_RTL; 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) // heading Current heading in degrees, in compass units (0..360, 0=north)
DECIDEGREES_TO_DEGREES(attitude.values.yaw), DECIDEGREES_TO_DEGREES(attitude.values.yaw),
// throttle Current throttle setting in integer percent, 0 to 100 // 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) // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate)
mavAltitude, mavAltitude,
// climb Current climb rate in meters/second // climb Current climb rate in meters/second
@ -1102,9 +1104,9 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) { if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) {
// Only process scheduled data if we didn't serve any incoming request this cycle // 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) && (rxConfig()->serialrx_provider == SERIALRX_MAVLINK) &&
!tristateWithDefaultOnIsActive(rxConfig()->halfDuplex) !tristateWithDefaultOnIsActive(rxConfig()->halfDuplex)
) )

2
src/main/telemetry/sim.c
View file

@ -122,7 +122,7 @@ static uint8_t simModuleState = SIM_MODULE_NOT_DETECTED;
static int simRssi; static int simRssi;
static uint8_t accEvent = ACC_EVENT_NONE; static uint8_t accEvent = ACC_EVENT_NONE;
static char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " }; 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", "ATT" };
static const char gpsFixIndicators[] = { '!', '*', ' ' }; static const char gpsFixIndicators[] = { '!', '*', ' ' };
static bool checkGroundStationNumber(uint8_t* rv) static bool checkGroundStationNumber(uint8_t* rv)