From 221d8fe3ad43484d0ca865a5602b3c948188b91d Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 14 Sep 2023 15:20:43 +0100 Subject: [PATCH] fixes + telemetry --- src/main/fc/runtime_config.c | 2 + src/main/fc/runtime_config.h | 1 + src/main/flight/pid.c | 9 +-- src/main/io/osd_dji_hd.c | 1 + src/main/programming/logic_condition.c | 106 +++++++++++++------------ src/main/programming/logic_condition.h | 1 + src/main/telemetry/mavlink.c | 8 +- src/main/telemetry/sim.c | 2 +- 8 files changed, 70 insertions(+), 60 deletions(-) diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 15dadd1c59..13a54af0e6 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -173,6 +173,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void) if (FLIGHT_MODE(HORIZON_MODE)) return FLM_HORIZON; + if (FLIGHT_MODE(ATTIHOLD_MODE)) + return FLM_ATTIHOLD; return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO; } diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 8c2bddb0b4..cad5ec18f6 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -162,6 +162,7 @@ typedef enum { FLM_CRUISE, FLM_LAUNCH, FLM_FAILSAFE, + FLM_ATTIHOLD, FLM_COUNT } flightModeForTelemetry_e; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ec5076898f..f5b861080f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1110,8 +1110,11 @@ void FAST_CODE pidController(float dT) // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ATTI_MODE const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f; levelingEnabled = false; + 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++) { 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)) { static int16_t attiHoldTarget[2]; - attiModeActive = true; if (restartAttiMode) { // set target attitude to current attitude when initialised attiHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL]; @@ -1143,9 +1145,6 @@ void FAST_CODE pidController(float dT) levelingEnabled = true; } } - if (!restartAttiMode) { // set restart flag if attihold_mode not active on previous loop - restartAttiMode = !attiModeActive; - } // Apply Turn Assistance if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index e79ddc0dfd..165a909b16 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -300,6 +300,7 @@ static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask) case FLM_ALTITUDE_HOLD: case FLM_POSITION_HOLD: case FLM_MISSION: + case FLM_ATTIHOLD: default: // Unsupported ATM, keep at ANGLE bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index f07e487f9c..f8191e2da1 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -95,7 +95,7 @@ static int logicConditionCompute( int temporaryValue; #if defined(USE_VTX_CONTROL) vtxDeviceCapability_t vtxDeviceCapability; -#endif +#endif switch (operation) { @@ -152,7 +152,7 @@ static int logicConditionCompute( case LOGIC_CONDITION_NOR: return !(operandA || operandB); - break; + break; case LOGIC_CONDITION_NOT: return !operandA; @@ -168,7 +168,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; @@ -236,7 +236,7 @@ static int logicConditionCompute( gvSet(operandA, operandB); return operandB; break; - + case LOGIC_CONDITION_GVAR_INC: temporaryValue = gvGet(operandA) + operandB; gvSet(operandA, temporaryValue); @@ -268,7 +268,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; @@ -286,10 +286,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); @@ -337,18 +337,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); @@ -364,18 +364,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: @@ -385,11 +385,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; @@ -471,11 +471,11 @@ static int logicConditionCompute( } else { return false; } - break; - + break; + default: return false; - break; + break; } } @@ -484,7 +484,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 @@ -493,13 +493,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; /* @@ -574,7 +574,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; @@ -620,11 +620,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; @@ -632,7 +632,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; @@ -648,7 +648,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; @@ -660,7 +660,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; @@ -708,15 +708,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; @@ -724,7 +724,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; @@ -733,16 +733,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; @@ -776,14 +776,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; @@ -835,12 +835,16 @@ static int logicConditionGetFlightModeOperandValue(int operand) { return (bool) FLIGHT_MODE(HORIZON_MODE); break; + case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ATTIMODE: + return (bool) FLIGHT_MODE(ATTIHOLD_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) || + 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; @@ -879,7 +883,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: @@ -907,7 +911,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { retVal = programmingPidGetOutput(operand); } break; - + case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS: retVal = logicConditionGetWaypointOperandValue(operand); break; @@ -921,7 +925,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; @@ -1003,7 +1007,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 { diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 779dbb1b98..403ad1da87 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -154,6 +154,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_ATTIMODE, // 16 } logicFlightModeOperands_e; typedef enum { diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 90c1377bc8..6d28bdeea2 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -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_ATTIHOLD: 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_ATTIHOLD: 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) ) diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index bb5f8b0465..22624d1d63 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -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", "ATT" }; static const char gpsFixIndicators[] = { '!', '*', ' ' }; static bool checkGroundStationNumber(uint8_t* rv)