static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs) { uint32_t startTime = 0; if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} // PID - note this is function pointer set by setPIDController() pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs); DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime); #ifdef USE_RUNAWAY_TAKEOFF // Check to see if runaway takeoff detection is active (anti-taz), the pidSum is over the threshold, // and gyro rate for any axis is above the limit for at least the activate delay period. // If so, disarm for safety if (ARMING_FLAG(ARMED) && !STATE(FIXED_WING) && pidConfig()->runaway_takeoff_prevention && !runawayTakeoffCheckDisabled && !flipOverAfterCrashMode && !runawayTakeoffTemporarilyDisabled && (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) { if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) || (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) || (fabsf(pidData[FD_YAW].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)) && ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) || (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) || (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) { if (runawayTakeoffTriggerUs == 0) { runawayTakeoffTriggerUs = currentTimeUs + RUNAWAY_TAKEOFF_ACTIVATE_DELAY; } else if (currentTimeUs > runawayTakeoffTriggerUs) { setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF); disarm(); } } else { runawayTakeoffTriggerUs = 0; } DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_TRUE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, runawayTakeoffTriggerUs == 0 ? DEBUG_RUNAWAY_TAKEOFF_FALSE : DEBUG_RUNAWAY_TAKEOFF_TRUE); } else { runawayTakeoffTriggerUs = 0; DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_FALSE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE); } #endif #ifdef USE_PID_AUDIO if (isModeActivationConditionPresent(BOXPIDAUDIO)) { pidAudioUpdate(); } #endif }
void processRx(void) { static bool armedBeeperOn = false; static bool airmodeIsActivated; calculateRxChannelsAndUpdateFailsafe(currentTime); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) mwDisarm(); } updateRSSI(currentTime); if (feature(FEATURE_FAILSAFE)) { if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { failsafeStartMonitoring(); } failsafeUpdateState(); } throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset } else { airmodeIsActivated = false; } /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { pidResetErrorGyroState(); if (currentProfile->pidProfile.pidAtMinThrottle) pidStabilisationState(PID_STABILISATION_ON); else pidStabilisationState(PID_STABILISATION_OFF); } else { pidStabilisationState(PID_STABILISATION_ON); } // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) && !feature(FEATURE_3D) && !isAirmodeActive() ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if (masterConfig.auto_disarm_delay != 0 && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over mwDisarm(); armedBeeperOn = false; } else { // still armed; do warning beeps while armed beeper(BEEPER_ARMED); armedBeeperOn = true; } } else { // throttle is not low if (masterConfig.auto_disarm_delay != 0) { // extend disarm time disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; } if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } else { // arming is via AUX switch; beep while throttle low if (throttleStatus == THROTTLE_LOW) { beeper(BEEPER_ARMED); armedBeeperOn = true; } else if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.disarm_kill_switch); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } updateActivatedModes(masterConfig.modeActivationConditions); if (!cliMode) { updateAdjustmentStates(masterConfig.adjustmentRanges); processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig); } bool canUseHorizonMode = true; if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { LED1_OFF; } #ifdef MAG if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading } } #endif #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsWaypointsAndMode(); } #endif if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { if ((!masterConfig.telemetryConfig.telemetry_switch && ARMING_FLAG(ARMED)) || (masterConfig.telemetryConfig.telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) { releaseSharedTelemetryPorts(); } else { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspAllocateSerialPorts(&masterConfig.serialConfig); } } #endif #ifdef VTX vtxUpdateActivatedChannel(); #endif }
void mixTable(pidProfile_t *pidProfile) { uint32_t i = 0; float vbatCompensationFactor = 1; // Scale roll/pitch/yaw uniformly to fit within throttle range // Initial mixer concept by bdoiron74 reused and optimized for Air Mode float motorMix[MAX_SUPPORTED_MOTORS], motorMixMax = 0, motorMixMin = 0; float throttleRange = 0, throttle = 0; float motorOutputRange, motorMixRange; uint16_t motorOutputMin, motorOutputMax; static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions // Find min and max throttle based on condition. if (feature(FEATURE_3D)) { if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling motorOutputMax = deadbandMotor3dLow; motorOutputMin = minMotorOutputNormal; throttlePrevious = rcCommand[THROTTLE]; throttle = rcCommand[THROTTLE] + flight3DConfig->deadband3d_throttle; } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling motorOutputMax = maxMotorOutputNormal; motorOutputMin = deadbandMotor3dHigh; throttlePrevious = rcCommand[THROTTLE]; throttle = rcCommand[THROTTLE] - flight3DConfig->deadband3d_throttle; } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive throttle = deadbandMotor3dLow; motorOutputMin = motorOutputMax = minMotorOutputNormal; } else { // Deadband handling from positive to negative motorOutputMax = maxMotorOutputNormal; throttle = motorOutputMin = deadbandMotor3dHigh; } } else { throttle = rcCommand[THROTTLE]; motorOutputMin = minMotorOutputNormal; motorOutputMax = maxMotorOutputNormal; } motorOutputRange = motorOutputMax - motorOutputMin; throttle = constrainf((throttle - rxConfig->mincheck) / rcCommandThrottleRange, 0.0f, 1.0f); throttleRange = motorOutputMax - motorOutputMin; float scaledAxisPIDf[3]; // Limit the PIDsum for (int axis = 0; axis < 3; axis++) scaledAxisPIDf[axis] = constrainf(axisPIDf[axis] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); // Calculate voltage compensation if (batteryConfig && pidProfile->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation(); // Find roll/pitch/yaw desired output for (i = 0; i < motorCount; i++) { motorMix[i] = scaledAxisPIDf[PITCH] * currentMixer[i].pitch + scaledAxisPIDf[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_motor_direction * scaledAxisPIDf[YAW] * currentMixer[i].yaw; if (vbatCompensationFactor > 1.0f) motorMix[i] *= vbatCompensationFactor; // Add voltage compensation if (motorMix[i] > motorMixMax) motorMixMax = motorMix[i]; if (motorMix[i] < motorMixMin) motorMixMin = motorMix[i]; } motorMixRange = motorMixMax - motorMixMin; if (motorMixRange > 1.0f) { for (i = 0; i < motorCount; i++) { motorMix[i] /= motorMixRange; } // Get the maximum correction by setting offset to center throttle = 0.5f; } else { float throttleLimitOffset = motorMixRange / 2.0f; throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset); } // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (i = 0; i < motorCount; i++) { motor[i] = lrintf( motorOutputMin + (motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))) ); if (failsafeIsActive()) { if (isMotorProtocolDshot()) motor[i] = (motor[i] < motorOutputMin) ? disarmMotorOutput : motor[i]; // Prevent getting into special reserved range motor[i] = constrain(motor[i], disarmMotorOutput, motorOutputMax); } else { motor[i] = constrain(motor[i], motorOutputMin, motorOutputMax); } // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { if (((rcData[THROTTLE]) < rxConfig->mincheck)) { motor[i] = disarmMotorOutput; } } } // Anti Desync feature for ESC's. Limit rapid throttle changes if (motorConfig->maxEscThrottleJumpMs) { const int16_t maxThrottleStep = constrain(motorConfig->maxEscThrottleJumpMs / (1000 / targetPidLooptime), 2, 10000); // Only makes sense when it's within the range if (maxThrottleStep < throttleRange) { static int16_t motorPrevious[MAX_SUPPORTED_MOTORS]; motor[i] = constrain(motor[i], motorOutputMin, motorPrevious[i] + maxThrottleStep); // Only limit accelerating situation motorPrevious[i] = motor[i]; } } // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } } }
void mixTable(void *pidProfilePtr) { uint32_t i = 0; fix12_t vbatCompensationFactor = 0; static fix12_t mixReduction; bool use_vbat_compensation = false; pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr; if (batteryConfig && pidProfile->vbatPidCompensation) { use_vbat_compensation = true; vbatCompensationFactor = calculateVbatPidCompensation(); } bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code // Initial mixer concept by bdoiron74 reused and optimized for Air Mode int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS]; int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero. int16_t rollPitchYawMixMin = 0; if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation // Find roll/pitch/yaw desired output for (i = 0; i < motorCount; i++) { rollPitchYawMix[i] = axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw; if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i]; if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i]; if (debugMode == DEBUG_MIXER && i < 5) debug[i] = rollPitchYawMix[i]; } // Scale roll/pitch/yaw uniformly to fit within throttle range int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin; int16_t throttleRange, throttle; int16_t throttleMin, throttleMax; static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions // Find min and max throttle based on condition. if (feature(FEATURE_3D)) { if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; throttlePrevious = throttle = rcCommand[THROTTLE]; } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling throttleMax = escAndServoConfig->maxthrottle; throttleMin = flight3DConfig->deadband3d_high; throttlePrevious = throttle = rcCommand[THROTTLE]; } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive throttle = throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; } else { // Deadband handling from positive to negative throttleMax = escAndServoConfig->maxthrottle; throttle = throttleMin = flight3DConfig->deadband3d_high; } } else { throttle = rcCommand[THROTTLE]; throttleMin = escAndServoConfig->minthrottle; throttleMax = escAndServoConfig->maxthrottle; } throttleRange = throttleMax - throttleMin; if (rollPitchYawMixRange > throttleRange) { mixReduction = qConstruct(throttleRange, rollPitchYawMixRange); for (i = 0; i < motorCount; i++) { rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]); } // Get the maximum correction by setting offset to center throttleMin = throttleMax = throttleMin + (throttleRange / 2); } else { throttleMin = throttleMin + (rollPitchYawMixRange / 2); throttleMax = throttleMax - (rollPitchYawMixRange / 2); } // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (i = 0; i < motorCount; i++) { motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax); if (isFailsafeActive) { motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); } else if (feature(FEATURE_3D)) { if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) { motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low); } else { motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle); } } else { motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); } // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { if (((rcData[THROTTLE]) < rxConfig->mincheck)) { motor[i] = escAndServoConfig->mincommand; } } } // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } } // motor outputs are used as sources for servo mixing, so motors must be calculated before servos. #if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS) // airplane / servo mixes switch (currentMixerMode) { case MIXER_CUSTOM_AIRPLANE: case MIXER_FLYING_WING: case MIXER_AIRPLANE: case MIXER_BICOPTER: case MIXER_CUSTOM_TRI: case MIXER_TRI: case MIXER_DUALCOPTER: case MIXER_SINGLECOPTER: case MIXER_GIMBAL: servoMixer(); break; /* case MIXER_GIMBAL: servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); break; */ default: break; } // camera stabilization if (feature(FEATURE_SERVO_TILT)) { // center at fixed position, or vary either pitch or roll by RC channel servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } else { servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } } } // constrain servos for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values } #endif }
void osdDrawSingleElement(uint8_t item) { if (!VISIBLE(OSD_cfg.item_pos[item]) || BLINK(OSD_cfg.item_pos[item])) return; uint8_t elemPosX = OSD_X(OSD_cfg.item_pos[item]); uint8_t elemPosY = OSD_Y(OSD_cfg.item_pos[item]); char buff[32]; switch(item) { case OSD_RSSI_VALUE: { uint16_t osdRssi = rssi * 100 / 1024; // change range if (osdRssi >= 100) osdRssi = 99; buff[0] = SYM_RSSI; sprintf(buff + 1, "%d", osdRssi); break; } case OSD_MAIN_BATT_VOLTAGE: { buff[0] = SYM_BATT_5; sprintf(buff + 1, "%d.%1dV", vbat / 10, vbat % 10); break; } case OSD_CURRENT_DRAW: { buff[0] = SYM_AMP; sprintf(buff + 1, "%d.%02d", abs(amperage) / 100, abs(amperage) % 100); break; } case OSD_MAH_DRAWN: { buff[0] = SYM_MAH; sprintf(buff + 1, "%d", mAhDrawn); break; } #ifdef GPS case OSD_GPS_SATS: { buff[0] = 0x1f; sprintf(buff + 1, "%d", GPS_numSat); break; } case OSD_GPS_SPEED: { sprintf(buff, "%d", GPS_speed * 36 / 1000); break; } #endif // GPS case OSD_ALTITUDE: { int32_t alt = osdGetAltitude(BaroAlt); sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); break; } case OSD_ONTIME: { uint32_t seconds = micros() / 1000000; buff[0] = SYM_ON_M; sprintf(buff + 1, "%02d:%02d", seconds / 60, seconds % 60); break; } case OSD_FLYTIME: { buff[0] = SYM_FLY_M; sprintf(buff + 1, "%02d:%02d", flyTime / 60, flyTime % 60); break; } case OSD_FLYMODE: { char *p = "ACRO"; if (isAirmodeActive()) p = "AIR"; if (FLIGHT_MODE(FAILSAFE_MODE)) p = "!FS"; else if (FLIGHT_MODE(ANGLE_MODE)) p = "STAB"; else if (FLIGHT_MODE(HORIZON_MODE)) p = "HOR"; max7456Write(elemPosX, elemPosY, p); return; } case OSD_CRAFT_NAME: { if (strlen(masterConfig.name) == 0) strcpy(buff, "CRAFT_NAME"); else { for (uint8_t i = 0; i < MAX_NAME_LENGTH; i++) { buff[i] = toupper((unsigned char)masterConfig.name[i]); if (masterConfig.name[i] == 0) break; } } break; } case OSD_THROTTLE_POS: { buff[0] = SYM_THR; buff[1] = SYM_THR1; sprintf(buff + 2, "%d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); break; } #ifdef VTX case OSD_VTX_CHANNEL: { sprintf(buff, "CH:%d", current_vtx_channel % CHANNELS_PER_BAND + 1); break; } #endif // VTX case OSD_CROSSHAIRS: { uint8_t *screenBuffer = max7456GetScreenBuffer(); uint16_t position = 194; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) position += 30; screenBuffer[position - 1] = (SYM_AH_CENTER_LINE); screenBuffer[position + 1] = (SYM_AH_CENTER_LINE_RIGHT); screenBuffer[position] = (SYM_AH_CENTER); return; } case OSD_ARTIFICIAL_HORIZON: { uint8_t *screenBuffer = max7456GetScreenBuffer(); uint16_t position = 194; int rollAngle = attitude.values.roll; int pitchAngle = attitude.values.pitch; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) position += 30; if (pitchAngle > AH_MAX_PITCH) pitchAngle = AH_MAX_PITCH; if (pitchAngle < -AH_MAX_PITCH) pitchAngle = -AH_MAX_PITCH; if (rollAngle > AH_MAX_ROLL) rollAngle = AH_MAX_ROLL; if (rollAngle < -AH_MAX_ROLL) rollAngle = -AH_MAX_ROLL; for (uint8_t x = 0; x <= 8; x++) { int y = (rollAngle * (4 - x)) / 64; y -= pitchAngle / 8; y += 41; if (y >= 0 && y <= 81) { uint16_t pos = position - 7 + LINE * (y / 9) + 3 - 4 * LINE + x; screenBuffer[pos] = (SYM_AH_BAR9_0 + (y % 9)); } } osdDrawSingleElement(OSD_HORIZON_SIDEBARS); return; } case OSD_HORIZON_SIDEBARS: { uint8_t *screenBuffer = max7456GetScreenBuffer(); uint16_t position = 194; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) position += 30; // Draw AH sides int8_t hudwidth = AH_SIDEBAR_WIDTH_POS; int8_t hudheight = AH_SIDEBAR_HEIGHT_POS; for (int8_t x = -hudheight; x <= hudheight; x++) { screenBuffer[position - hudwidth + (x * LINE)] = (SYM_AH_DECORATION); screenBuffer[position + hudwidth + (x * LINE)] = (SYM_AH_DECORATION); } // AH level indicators screenBuffer[position - hudwidth + 1] = (SYM_AH_LEFT); screenBuffer[position + hudwidth - 1] = (SYM_AH_RIGHT); return; } default: return; } max7456Write(elemPosX, elemPosY, buff); }
FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation) { if (isFlipOverAfterCrashMode()) { applyFlipOverAfterCrashModeToMotors(); return; } // Find min and max throttle based on conditions. Throttle has to be known before mixing calculateThrottleAndCurrentMotorEndpoints(currentTimeUs); // Calculate and Limit the PID sum const float scaledAxisPidRoll = constrainf(pidData[FD_ROLL].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; const float scaledAxisPidPitch = constrainf(pidData[FD_PITCH].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; uint16_t yawPidSumLimit = currentPidProfile->pidSumLimitYaw; #ifdef USE_YAW_SPIN_RECOVERY const bool yawSpinDetected = gyroYawSpinDetected(); if (yawSpinDetected) { yawPidSumLimit = PIDSUM_LIMIT_MAX; // Set to the maximum limit during yaw spin recovery to prevent limiting motor authority } #endif // USE_YAW_SPIN_RECOVERY float scaledAxisPidYaw = constrainf(pidData[FD_YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING; if (!mixerConfig()->yaw_motors_reversed) { scaledAxisPidYaw = -scaledAxisPidYaw; } // Calculate voltage compensation const float vbatCompensationFactor = vbatPidCompensation ? calculateVbatPidCompensation() : 1.0f; // Apply the throttle_limit_percent to scale or limit the throttle based on throttle_limit_type if (currentControlRateProfile->throttle_limit_type != THROTTLE_LIMIT_TYPE_OFF) { throttle = applyThrottleLimit(throttle); } #ifdef USE_YAW_SPIN_RECOVERY // 50% throttle provides the maximum authority for yaw recovery when airmode is not active. // When airmode is active the throttle setting doesn't impact recovery authority. if (yawSpinDetected && !isAirmodeActive()) { throttle = 0.5f; // } #endif // USE_YAW_SPIN_RECOVERY // Find roll/pitch/yaw desired output float motorMix[MAX_SUPPORTED_MOTORS]; float motorMixMax = 0, motorMixMin = 0; for (int i = 0; i < motorCount; i++) { float mix = scaledAxisPidRoll * currentMixer[i].roll + scaledAxisPidPitch * currentMixer[i].pitch + scaledAxisPidYaw * currentMixer[i].yaw; mix *= vbatCompensationFactor; // Add voltage compensation if (mix > motorMixMax) { motorMixMax = mix; } else if (mix < motorMixMin) { motorMixMin = mix; } motorMix[i] = mix; } pidUpdateAntiGravityThrottleFilter(throttle); #if defined(USE_THROTTLE_BOOST) if (throttleBoost > 0.0f) { const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle); throttle = constrainf(throttle + throttleBoost * throttleHpf, 0.0f, 1.0f); } #endif #ifdef USE_GPS_RESCUE // If gps rescue is active then override the throttle. This prevents things // like throttle boost or throttle limit from negatively affecting the throttle. if (FLIGHT_MODE(GPS_RESCUE_MODE)) { throttle = gpsRescueGetThrottle(); } #endif motorMixRange = motorMixMax - motorMixMin; if (motorMixRange > 1.0f) { for (int i = 0; i < motorCount; i++) { motorMix[i] /= motorMixRange; } // Get the maximum correction by setting offset to center when airmode enabled if (isAirmodeActive()) { throttle = 0.5f; } } else { if (isAirmodeActive() || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle const float throttleLimitOffset = motorMixRange / 2.0f; throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset); } } // Apply the mix to motor endpoints applyMixToMotors(motorMix); }
static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) { // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (int i = 0; i < motorCount; i++) { float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle)); #ifdef USE_SERVOS if (mixerIsTricopter()) { motorOutput += mixerTricopterMotorCorrection(i); } #endif if (failsafeIsActive()) { if (isMotorProtocolDshot()) { motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range } motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax); } else { motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax); } // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { if (((rcData[THROTTLE]) < rxConfig()->mincheck)) { motorOutput = disarmMotorOutput; } } motor[i] = motorOutput; } // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (int i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } } }
static bool osdDrawSingleElement(uint8_t item) { if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) { return false; } uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]); uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]); char buff[OSD_ELEMENT_BUFFER_LENGTH] = ""; switch (item) { case OSD_RSSI_VALUE: { uint16_t osdRssi = getRssi() * 100 / 1024; // change range if (osdRssi >= 100) osdRssi = 99; tfp_sprintf(buff, "%c%2d", SYM_RSSI, osdRssi); break; } case OSD_MAIN_BATT_VOLTAGE: buff[0] = osdGetBatterySymbol(osdGetBatteryAverageCellVoltage()); tfp_sprintf(buff + 1, "%2d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT); break; case OSD_CURRENT_DRAW: { const int32_t amperage = getAmperage(); tfp_sprintf(buff, "%3d.%02d%c", abs(amperage) / 100, abs(amperage) % 100, SYM_AMP); break; } case OSD_MAH_DRAWN: tfp_sprintf(buff, "%4d%c", getMAhDrawn(), SYM_MAH); break; #ifdef USE_GPS case OSD_GPS_SATS: tfp_sprintf(buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat); break; case OSD_GPS_SPEED: // FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K (M for MPH) switch (osdConfig()->units) { case OSD_UNIT_IMPERIAL: tfp_sprintf(buff, "%3dM", CM_S_TO_MPH(gpsSol.groundSpeed)); break; default: tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed)); break; } break; case OSD_GPS_LAT: // The SYM_LAT symbol in the actual font contains only blank, so we use the SYM_ARROW_NORTH osdFormatCoordinate(buff, SYM_ARROW_NORTH, gpsSol.llh.lat); break; case OSD_GPS_LON: // The SYM_LON symbol in the actual font contains only blank, so we use the SYM_ARROW_EAST osdFormatCoordinate(buff, SYM_ARROW_EAST, gpsSol.llh.lon); break; case OSD_HOME_DIR: if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { if (GPS_distanceToHome > 0) { const int h = GPS_directionToHome - DECIDEGREES_TO_DEGREES(attitude.values.yaw); buff[0] = osdGetDirectionSymbolFromHeading(h); } else { // We don't have a HOME symbol in the font, by now we use this buff[0] = SYM_THR1; } } else { // We use this symbol when we don't have a FIX buff[0] = SYM_COLON; } buff[1] = 0; break; case OSD_HOME_DIST: if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { const int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceToHome); tfp_sprintf(buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol()); } else { // We use this symbol when we don't have a FIX buff[0] = SYM_COLON; // overwrite any previous distance with blanks memset(buff + 1, SYM_BLANK, 6); buff[7] = '\0'; } break; #endif // GPS case OSD_COMPASS_BAR: memcpy(buff, compassBar + osdGetHeadingIntoDiscreteDirections(DECIDEGREES_TO_DEGREES(attitude.values.yaw), 16), 9); buff[9] = 0; break; case OSD_ALTITUDE: osdFormatAltitudeString(buff, getEstimatedAltitude()); break; case OSD_ITEM_TIMER_1: case OSD_ITEM_TIMER_2: osdFormatTimer(buff, true, true, item - OSD_ITEM_TIMER_1); break; case OSD_REMAINING_TIME_ESTIMATE: { const int mAhDrawn = getMAhDrawn(); const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)flyTime) / mAhDrawn); if (mAhDrawn < 0.1 * osdConfig()->cap_alarm) { tfp_sprintf(buff, "--:--"); } else if (mAhDrawn > osdConfig()->cap_alarm) { tfp_sprintf(buff, "00:00"); } else { osdFormatTime(buff, OSD_TIMER_PREC_SECOND, remaining_time); } break; } case OSD_FLYMODE: { if (FLIGHT_MODE(FAILSAFE_MODE)) { strcpy(buff, "!FS!"); } else if (FLIGHT_MODE(ANGLE_MODE)) { strcpy(buff, "STAB"); } else if (FLIGHT_MODE(HORIZON_MODE)) { strcpy(buff, "HOR "); } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) { strcpy(buff, "RESC"); } else if (isAirmodeActive()) { strcpy(buff, "AIR "); } else { strcpy(buff, "ACRO"); } break; } case OSD_ANTI_GRAVITY: { if (pidItermAccelerator() > 1.0f) { strcpy(buff, "AG"); } break; } case OSD_CRAFT_NAME: // This does not strictly support iterative updating if the craft name changes at run time. But since the craft name is not supposed to be changing this should not matter, and blanking the entire length of the craft name string on update will make it impossible to configure elements to be displayed on the right hand side of the craft name. //TODO: When iterative updating is implemented, change this so the craft name is only printed once whenever the OSD 'flight' screen is entered. if (strlen(pilotConfig()->name) == 0) { strcpy(buff, "CRAFT_NAME"); } else { unsigned i; for (i = 0; i < MAX_NAME_LENGTH; i++) { if (pilotConfig()->name[i]) { buff[i] = toupper((unsigned char)pilotConfig()->name[i]); } else { break; } } buff[i] = '\0'; } break; case OSD_THROTTLE_POS: buff[0] = SYM_THR; buff[1] = SYM_THR1; tfp_sprintf(buff + 2, "%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); break; #if defined(USE_VTX_COMMON) case OSD_VTX_CHANNEL: { const char vtxBandLetter = vtx58BandLetter[vtxSettingsConfig()->band]; const char *vtxChannelName = vtx58ChannelNames[vtxSettingsConfig()->channel]; uint8_t vtxPower = vtxSettingsConfig()->power; const vtxDevice_t *vtxDevice = vtxCommonDevice(); if (vtxDevice && vtxSettingsConfig()->lowPowerDisarm) { vtxCommonGetPowerIndex(vtxDevice, &vtxPower); } tfp_sprintf(buff, "%c:%s:%1d", vtxBandLetter, vtxChannelName, vtxPower); break; } #endif case OSD_CROSSHAIRS: buff[0] = SYM_AH_CENTER_LINE; buff[1] = SYM_AH_CENTER; buff[2] = SYM_AH_CENTER_LINE_RIGHT; buff[3] = 0; break; case OSD_ARTIFICIAL_HORIZON: { // Get pitch and roll limits in tenths of degrees const int maxPitch = osdConfig()->ahMaxPitch * 10; const int maxRoll = osdConfig()->ahMaxRoll * 10; const int rollAngle = constrain(attitude.values.roll, -maxRoll, maxRoll); int pitchAngle = constrain(attitude.values.pitch, -maxPitch, maxPitch); // Convert pitchAngle to y compensation value // (maxPitch / 25) divisor matches previous settings of fixed divisor of 8 and fixed max AHI pitch angle of 20.0 degrees pitchAngle = ((pitchAngle * 25) / maxPitch) - 41; // 41 = 4 * AH_SYMBOL_COUNT + 5 for (int x = -4; x <= 4; x++) { const int y = ((-rollAngle * x) / 64) - pitchAngle; if (y >= 0 && y <= 81) { displayWriteChar(osdDisplayPort, elemPosX + x, elemPosY + (y / AH_SYMBOL_COUNT), (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT))); } } return true; } case OSD_HORIZON_SIDEBARS: { // Draw AH sides const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS; const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS; for (int y = -hudheight; y <= hudheight; y++) { displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY + y, SYM_AH_DECORATION); displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY + y, SYM_AH_DECORATION); } // AH level indicators displayWriteChar(osdDisplayPort, elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT); displayWriteChar(osdDisplayPort, elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT); return true; } case OSD_ROLL_PIDS: osdFormatPID(buff, "ROL", ¤tPidProfile->pid[PID_ROLL]); break; case OSD_PITCH_PIDS: osdFormatPID(buff, "PIT", ¤tPidProfile->pid[PID_PITCH]); break; case OSD_YAW_PIDS: osdFormatPID(buff, "YAW", ¤tPidProfile->pid[PID_YAW]); break; case OSD_POWER: tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 1000); break; case OSD_PIDRATE_PROFILE: tfp_sprintf(buff, "%d-%d", getCurrentPidProfileIndex() + 1, getCurrentControlRateProfileIndex() + 1); break; case OSD_WARNINGS: { #define OSD_WARNINGS_MAX_SIZE 11 #define OSD_FORMAT_MESSAGE_BUFFER_SIZE (OSD_WARNINGS_MAX_SIZE + 1) STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= sizeof(buff), osd_warnings_size_exceeds_buffer_size); const batteryState_e batteryState = getBatteryState(); if (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) && batteryState == BATTERY_CRITICAL) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " LAND NOW"); break; } #ifdef USE_ADC_INTERNAL uint8_t coreTemperature = getCoreTemperatureCelsius(); if (osdWarnGetState(OSD_WARNING_CORE_TEMPERATURE) && coreTemperature >= osdConfig()->core_temp_alarm) { char coreTemperatureWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE]; tfp_sprintf(coreTemperatureWarningMsg, "CORE: %3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius() * 10) / 10, osdGetTemperatureSymbolForSelectedUnit()); osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, coreTemperatureWarningMsg); break; } #endif #ifdef USE_ESC_SENSOR // Show warning if we lose motor output, the ESC is overheating or excessive current draw if (feature(FEATURE_ESC_SENSOR) && osdWarnGetState(OSD_WARNING_ESC_FAIL)) { char escWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE]; unsigned pos = 0; const char *title = "ESC"; // center justify message while (pos < (OSD_WARNINGS_MAX_SIZE - (strlen(title) + getMotorCount())) / 2) { escWarningMsg[pos++] = ' '; } strcpy(escWarningMsg + pos, title); pos += strlen(title); unsigned i = 0; unsigned escWarningCount = 0; while (i < getMotorCount() && pos < OSD_FORMAT_MESSAGE_BUFFER_SIZE - 1) { escSensorData_t *escData = getEscSensorData(i); const char motorNumber = '1' + i; // if everything is OK just display motor number else R, T or C char warnFlag = motorNumber; if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && calcEscRpm(escData->rpm) <= osdConfig()->esc_rpm_alarm) { warnFlag = 'R'; } if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) { warnFlag = 'T'; } if (ARMING_FLAG(ARMED) && osdConfig()->esc_current_alarm != ESC_CURRENT_ALARM_OFF && escData->current >= osdConfig()->esc_current_alarm) { warnFlag = 'C'; } escWarningMsg[pos++] = warnFlag; if (warnFlag != motorNumber) { escWarningCount++; } i++; } escWarningMsg[pos] = '\0'; if (escWarningCount > 0) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, escWarningMsg); } break; } #endif // Warn when in flip over after crash mode if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && isFlipOverAfterCrashMode()) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "CRASH FLIP"); break; } // Show most severe reason for arming being disabled if (osdWarnGetState(OSD_WARNING_ARMING_DISABLE) && IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) { const armingDisableFlags_e flags = getArmingDisableFlags(); for (int i = 0; i < ARMING_DISABLE_FLAGS_COUNT; i++) { if (flags & (1 << i)) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDisableFlagNames[i]); break; } } break; } if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LOW BATTERY"); break; } // Show warning if battery is not fresh if (osdWarnGetState(OSD_WARNING_BATTERY_NOT_FULL) && !ARMING_FLAG(WAS_EVER_ARMED) && (getBatteryState() == BATTERY_OK) && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "BATT < FULL"); break; } // Visual beeper if (osdWarnGetState(OSD_WARNING_VISUAL_BEEPER) && showVisualBeeper) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " * * * *"); break; } osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, NULL); break; } case OSD_AVG_CELL_VOLTAGE: { const int cellV = osdGetBatteryAverageCellVoltage(); buff[0] = osdGetBatterySymbol(cellV); tfp_sprintf(buff + 1, "%d.%02d%c", cellV / 100, cellV % 100, SYM_VOLT); break; } case OSD_DEBUG: tfp_sprintf(buff, "DBG %5d %5d %5d %5d", debug[0], debug[1], debug[2], debug[3]); break; case OSD_PITCH_ANGLE: case OSD_ROLL_ANGLE: { const int angle = (item == OSD_PITCH_ANGLE) ? attitude.values.pitch : attitude.values.roll; tfp_sprintf(buff, "%c%02d.%01d", angle < 0 ? '-' : ' ', abs(angle / 10), abs(angle % 10)); break; } case OSD_MAIN_BATT_USAGE: { // Set length of indicator bar #define MAIN_BATT_USAGE_STEPS 11 // Use an odd number so the bar can be centered. // Calculate constrained value const float value = constrain(batteryConfig()->batteryCapacity - getMAhDrawn(), 0, batteryConfig()->batteryCapacity); // Calculate mAh used progress const uint8_t mAhUsedProgress = ceilf((value / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS))); // Create empty battery indicator bar buff[0] = SYM_PB_START; for (int i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) { buff[i] = i <= mAhUsedProgress ? SYM_PB_FULL : SYM_PB_EMPTY; } buff[MAIN_BATT_USAGE_STEPS + 1] = SYM_PB_CLOSE; if (mAhUsedProgress > 0 && mAhUsedProgress < MAIN_BATT_USAGE_STEPS) { buff[1 + mAhUsedProgress] = SYM_PB_END; } buff[MAIN_BATT_USAGE_STEPS+2] = '\0'; break; } case OSD_DISARMED: if (!ARMING_FLAG(ARMED)) { tfp_sprintf(buff, "DISARMED"); } else { if (!lastArmState) { // previously disarmed - blank out the message one time tfp_sprintf(buff, " "); } } break; case OSD_NUMERICAL_HEADING: { const int heading = DECIDEGREES_TO_DEGREES(attitude.values.yaw); tfp_sprintf(buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading); break; } case OSD_NUMERICAL_VARIO: { const int verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario()); const char directionSymbol = verticalSpeed < 0 ? SYM_ARROW_SOUTH : SYM_ARROW_NORTH; tfp_sprintf(buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10)); break; } #ifdef USE_ESC_SENSOR case OSD_ESC_TMP: if (feature(FEATURE_ESC_SENSOR)) { tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(escDataCombined->temperature * 10) / 10, osdGetTemperatureSymbolForSelectedUnit()); } break; case OSD_ESC_RPM: if (feature(FEATURE_ESC_SENSOR)) { tfp_sprintf(buff, "%5d", escDataCombined == NULL ? 0 : calcEscRpm(escDataCombined->rpm)); } break; #endif #ifdef USE_RTC_TIME case OSD_RTC_DATETIME: osdFormatRtcDateTime(&buff[0]); break; #endif #ifdef USE_OSD_ADJUSTMENTS case OSD_ADJUSTMENT_RANGE: tfp_sprintf(buff, "%s: %3d", adjustmentRangeName, adjustmentRangeValue); break; #endif #ifdef USE_ADC_INTERNAL case OSD_CORE_TEMPERATURE: tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius() * 10) / 10, osdGetTemperatureSymbolForSelectedUnit()); break; #endif default: return false; } displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); return true; }
/* * processRx called from taskUpdateRxMain */ bool processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) { return false; } // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) disarm(); } updateRSSI(currentTimeUs); if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { failsafeStartMonitoring(); } failsafeUpdateState(); const throttleStatus_e throttleStatus = calculateThrottleStatus(); const uint8_t throttlePercent = calculateThrottlePercent(); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (throttlePercent >= rxConfig()->airModeActivateThreshold) { airmodeIsActivated = true; // Prevent Iterm from being reset } } else { airmodeIsActivated = false; } /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { pidResetITerm(); if (currentPidProfile->pidAtMinThrottle) pidStabilisationState(PID_STABILISATION_ON); else pidStabilisationState(PID_STABILISATION_OFF); } else { pidStabilisationState(PID_STABILISATION_ON); } #ifdef USE_RUNAWAY_TAKEOFF // If runaway_takeoff_prevention is enabled, accumulate the amount of time that throttle // is above runaway_takeoff_deactivate_throttle with the any of the R/P/Y sticks deflected // to at least runaway_takeoff_stick_percent percent while the pidSum on all axis is kept low. // Once the amount of accumulated time exceeds runaway_takeoff_deactivate_delay then disable // prevention for the remainder of the battery. if (ARMING_FLAG(ARMED) && pidConfig()->runaway_takeoff_prevention && !runawayTakeoffCheckDisabled && !flipOverAfterCrashMode && !runawayTakeoffTemporarilyDisabled && !STATE(FIXED_WING)) { // Determine if we're in "flight" // - motors running // - throttle over runaway_takeoff_deactivate_throttle_percent // - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent // - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit bool inStableFlight = false; if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running? const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle; const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT); if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit)) && (fabsf(pidData[FD_PITCH].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) && (fabsf(pidData[FD_ROLL].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) && (fabsf(pidData[FD_YAW].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) { inStableFlight = true; if (runawayTakeoffDeactivateUs == 0) { runawayTakeoffDeactivateUs = currentTimeUs; } } } // If we're in flight, then accumulate the time and deactivate once it exceeds runaway_takeoff_deactivate_delay milliseconds if (inStableFlight) { if (runawayTakeoffDeactivateUs == 0) { runawayTakeoffDeactivateUs = currentTimeUs; } uint16_t deactivateDelay = pidConfig()->runaway_takeoff_deactivate_delay; // at high throttle levels reduce deactivation delay by 50% if (throttlePercent >= RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT) { deactivateDelay = deactivateDelay / 2; } if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > deactivateDelay * 1000) { runawayTakeoffCheckDisabled = true; } } else { if (runawayTakeoffDeactivateUs != 0) { runawayTakeoffAccumulatedUs += cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs); } runawayTakeoffDeactivateUs = 0; } if (runawayTakeoffDeactivateUs == 0) { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, runawayTakeoffAccumulatedUs / 1000); } else { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_TRUE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, (cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) / 1000); } } else { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, DEBUG_RUNAWAY_TAKEOFF_FALSE); } #endif // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) && !feature(FEATURE_3D) && !isAirmodeActive() ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if (armingConfig()->auto_disarm_delay != 0 && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over disarm(); armedBeeperOn = false; } else { // still armed; do warning beeps while armed beeper(BEEPER_ARMED); armedBeeperOn = true; } } else { // throttle is not low if (armingConfig()->auto_disarm_delay != 0) { // extend disarm time disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; } if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } else { // arming is via AUX switch; beep while throttle low if (throttleStatus == THROTTLE_LOW) { beeper(BEEPER_ARMED); armedBeeperOn = true; } else if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } processRcStickPositions(); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } updateActivatedModes(); #ifdef USE_DSHOT /* Enable beep warning when the crash flip mode is active */ if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { beeper(BEEPER_CRASH_FLIP_MODE); } #endif if (!cliMode) { updateAdjustmentStates(); processRcAdjustments(currentControlRateProfile); } bool canUseHorizonMode = true; if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } #ifdef USE_GPS_RESCUE if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE)) { if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE); } } else { DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE); } #endif if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; // increase frequency of attitude task to reduce drift when in angle or horizon mode rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500)); } else { LED1_OFF; rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100)); } if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) { DISABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM); } #if defined(USE_ACC) || defined(USE_MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { #if defined(USE_GPS) || defined(USE_MAG) if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } #endif if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { if (imuQuaternionHeadfreeOffsetSet()){ beeper(BEEPER_RX_SET); } } } #endif if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } #ifdef USE_TELEMETRY static bool sharedPortTelemetryEnabled = false; if (feature(FEATURE_TELEMETRY)) { bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY)); if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) { mspSerialReleaseSharedTelemetryPorts(); telemetryCheckState(); sharedPortTelemetryEnabled = true; } else if (!enableSharedPortTelemetry && sharedPortTelemetryEnabled) { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspSerialAllocatePorts(); sharedPortTelemetryEnabled = false; } } #endif #ifdef USE_VTX_CONTROL vtxUpdateActivatedChannel(); if (canUpdateVTX()) { handleVTXControlButton(); } #endif #ifdef USE_ACRO_TRAINER pidSetAcroTrainerState(IS_RC_MODE_ACTIVE(BOXACROTRAINER) && sensors(SENSOR_ACC)); #endif // USE_ACRO_TRAINER #ifdef USE_RC_SMOOTHING_FILTER if (ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) { beeper(BEEPER_RC_SMOOTHING_INIT_FAIL); } #endif pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY)); return true; }