void batteryInit(batteryConfig_t *initialBatteryConfig) { batteryConfig = initialBatteryConfig; uint32_t i; for (i = 0; i < BATTERY_SAMPLE_COUNT; i++) { updateBatteryVoltage(); delay((32 / BATTERY_SAMPLE_COUNT) * 10); } unsigned cells = (vbat / batteryConfig->vbatmaxcellvoltage) + 1; if(cells > 8) // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) cells = 8; batteryCellCount = cells; batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; }
void annexCode(void) { int32_t tmp, tmp2; int32_t axis, prop1, prop2; static uint8_t batteryWarningEnabled = false; static uint8_t vbatTimer = 0; static int32_t vbatCycleTime = 0; // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value if (rcData[THROTTLE] < currentProfile.tpa_breakpoint) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { prop2 = 100 - (uint16_t)currentProfile.dynThrPID * (rcData[THROTTLE] - currentProfile.tpa_breakpoint) / (2000 - currentProfile.tpa_breakpoint); } else { prop2 = 100 - currentProfile.dynThrPID; } } for (axis = 0; axis < 3; axis++) { tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { if (currentProfile.deadband) { if (tmp > currentProfile.deadband) { tmp -= currentProfile.deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.rollPitchRate * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } if (axis == YAW) { if (currentProfile.yaw_deadband) { if (tmp > currentProfile.yaw_deadband) { tmp -= currentProfile.yaw_deadband; } else { tmp = 0; } } rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.yawRate * abs(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)currentProfile.pidProfile.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)currentProfile.pidProfile.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)currentProfile.pidProfile.D8[axis] * prop1 / 100; 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] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] if (f.HEADFREE_MODE) { float radDiff = degreesToRadians(heading - headFreeModeHold); float cosDiff = cosf(radDiff); float sinDiff = sinf(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) { vbatCycleTime += cycleTime; if (!(++vbatTimer % VBATFREQ)) { if (feature(FEATURE_VBAT)) { updateBatteryVoltage(); batteryWarningEnabled = shouldSoundBatteryAlarm(); } if (feature(FEATURE_CURRENT_METER)) { updateCurrentMeter(vbatCycleTime); } vbatCycleTime = 0; } } beepcodeUpdateState(batteryWarningEnabled); if (f.ARMED) { LED0_ON; } else { if (isCalibrating()) { LED0_TOGGLE; f.OK_TO_ARM = 0; } f.OK_TO_ARM = 1; if (!f.SMALL_ANGLE) { f.OK_TO_ARM = 0; } if (rcOptions[BOXAUTOTUNE]) { f.OK_TO_ARM = 0; } if (f.OK_TO_ARM) { disableWarningLed(); } else { enableWarningLed(currentTime); } updateWarningLed(currentTime); } #ifdef TELEMETRY checkTelemetryState(); #endif handleSerial(); #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTime); } #endif // 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 updateBattery(void) { updateBatteryVoltage(); /* battery has just been connected*/ if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD_MV) { /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ batteryState = BATTERY_OK; /* wait for VBatt to stabilise then we can calc number of cells (using the filtered value takes a long time to ramp up) We only do this on the ground so don't care if we do block, not worse than original code anyway*/ delay(VBATTERY_STABLE_DELAY); updateBatteryVoltage(); unsigned cells = (batteryAdcToVoltage(vbatLatestADC) / batteryConfig->vbatmaxcellvoltage) + 1; if (cells > 8) { // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) cells = 8; } batteryCellCount = cells; batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; } /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD_MV */ else if (batteryState != BATTERY_NOT_PRESENT && vbat <= VBATT_PRESENT_THRESHOLD_MV) { batteryState = BATTERY_NOT_PRESENT; batteryCellCount = 0; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; } switch(batteryState) { case BATTERY_OK: if (vbat <= (batteryWarningVoltage - VBATT_HYSTERESIS)) { batteryState = BATTERY_WARNING; beeper(BEEPER_BAT_LOW); } break; case BATTERY_WARNING: if (vbat <= (batteryCriticalVoltage - VBATT_HYSTERESIS)) { batteryState = BATTERY_CRITICAL; beeper(BEEPER_BAT_CRIT_LOW); } else if (vbat > (batteryWarningVoltage + VBATT_HYSTERESIS)){ batteryState = BATTERY_OK; } else { beeper(BEEPER_BAT_LOW); } break; case BATTERY_CRITICAL: if (vbat > (batteryCriticalVoltage + VBATT_HYSTERESIS)){ batteryState = BATTERY_WARNING; beeper(BEEPER_BAT_LOW); } else { beeper(BEEPER_BAT_CRIT_LOW); } break; case BATTERY_NOT_PRESENT: break; } }
void annexCode(void) { int32_t tmp, tmp2; int32_t axis, prop1 = 0, prop2; static batteryState_e batteryState = BATTERY_OK; static uint8_t vbatTimer = 0; static int32_t vbatCycleTime = 0; // 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 (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; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else if (axis == YAW) { if (currentProfile->rcControlsConfig.yaw_deadband) { if (tmp > currentProfile->rcControlsConfig.yaw_deadband) { tmp -= currentProfile->rcControlsConfig.yaw_deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100; // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids if (axis == YAW) { PIDweight[axis] = 100; } else { PIDweight[axis] = prop2; } 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] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] if (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(heading - headFreeModeHold); float cosDiff = cosf(radDiff); float sinDiff = sinf(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) { vbatCycleTime += cycleTime; if (!(++vbatTimer % VBATFREQ)) { if (feature(FEATURE_VBAT)) { updateBatteryVoltage(); batteryState = calculateBatteryState(); //handle beepers for battery levels if (batteryState == BATTERY_CRITICAL) beeper(BEEPER_BAT_CRIT_LOW); //critically low battery else if (batteryState == BATTERY_WARNING) beeper(BEEPER_BAT_LOW); //low battery } if (feature(FEATURE_CURRENT_METER)) { updateCurrentMeter(vbatCycleTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); } vbatCycleTime = 0; } } beeperUpdate(); //call periodic beeper handler 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 (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } warningLedUpdate(); } #ifdef TELEMETRY telemetryCheckState(); #endif handleSerial(); #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTime); } #endif // 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); }