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); }
/* This function processes RX dependent coefficients when new RX commands are available Those are: TPA, throttle expo */ static void updateRcCommands(void) { int32_t prop2; // PITCH & ROLL only dynamic PID adjustment, depending on throttle value if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); } else { prop2 = 100 - currentControlRateProfile->dynThrPID; } } for (int axis = 0; axis < 3; axis++) { int32_t prop1; int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500); if (axis == ROLL || axis == PITCH) { if (rcControlsConfig()->deadband) { if (tmp > rcControlsConfig()->deadband) { tmp -= rcControlsConfig()->deadband; } else { tmp = 0; } } rcCommand[axis] = rcLookupPitchRoll(tmp); prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. 100 means 100% of the pids PIDweight[axis] = prop2; } else { if (rcControlsConfig()->yaw_deadband) { if (tmp > rcControlsConfig()->yaw_deadband) { tmp -= rcControlsConfig()->yaw_deadband; } else { tmp = 0; } } rcCommand[axis] = rcLookupYaw(tmp) * -rcControlsConfig()->yaw_control_direction; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; // YAW TPA disabled. PIDweight[axis] = 100; } #ifdef USE_PID_MW23 // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)pidProfile()->P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)pidProfile()->I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)pidProfile()->D8[axis] * prop1 / 100; #endif if (rcData[axis] < rxConfig()->midrc) { rcCommand[axis] = -rcCommand[axis]; } } int32_t tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000] rcCommand[THROTTLE] = rcLookupThrottle(tmp); if (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); float cosDiff = cos_approx(radDiff); float sinDiff = sin_approx(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } }