void annexCode(void) { int32_t throttleValue; // Compute ROLL PITCH and YAW command rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband); rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband); rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], currentControlRateProfile->rcYawExpo8, currentProfile->rcControlsConfig.yaw_deadband); //Compute THROTTLE command throttleValue = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); throttleValue = (uint32_t)(throttleValue - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] rcCommand[THROTTLE] = rcLookupThrottle(throttleValue); if (FLIGHT_MODE(HEADFREE_MODE)) { const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); const float cosDiff = cos_approx(radDiff); const float sinDiff = sin_approx(radDiff); const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!STATE(SMALL_ANGLE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } #if defined(NAV) if (naivationBlockArming()) { DISABLE_ARMING_FLAG(OK_TO_ARM); } #endif if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } warningLedUpdate(); } // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); }
void annexCode(void) { int32_t tmp; for (int axis = 0; axis < 3; axis++) { tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { if (currentProfile->rcControlsConfig.deadband) { if (tmp > currentProfile->rcControlsConfig.deadband) { tmp -= currentProfile->rcControlsConfig.deadband; } else { tmp = 0; } } rcCommand[axis] = rcLookupPitchRoll(tmp); } else if (axis == YAW) { if (currentProfile->rcControlsConfig.yaw_deadband) { if (tmp > currentProfile->rcControlsConfig.yaw_deadband) { tmp -= currentProfile->rcControlsConfig.yaw_deadband; } else { tmp = 0; } } rcCommand[axis] = rcLookupYaw(tmp) * -1; } if (rcData[axis] < masterConfig.rxConfig.midrc) { rcCommand[axis] = -rcCommand[axis]; } } tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] rcCommand[THROTTLE] = rcLookupThrottle(tmp); if (FLIGHT_MODE(HEADFREE_MODE)) { const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); const float cosDiff = cos_approx(radDiff); const float sinDiff = sin_approx(radDiff); const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!STATE(SMALL_ANGLE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } #if defined(NAV) if (naivationBlockArming()) { DISABLE_ARMING_FLAG(OK_TO_ARM); } #endif if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } warningLedUpdate(); } // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); }