void batteryInit(void) { uint32_t i; uint32_t voltage = 0; // average up some voltage readings for (i = 0; i < 32; i++) { voltage += adcGetBattery(); delay(10); } voltage = batteryAdcToVoltage((uint16_t)(voltage / 32)); // autodetect cell count, going from 2S..6S for (i = 2; i < 6; i++) { if (voltage < i * cfg.vbatmaxcellvoltage) break; } batteryCellCount = i; batteryWarningVoltage = i * cfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI }
// this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds void annexCode(void) { static uint32_t calibratedAccTime; uint16_t tmp, tmp2; static uint8_t buzzerFreq; //delay between buzzer ring static uint8_t vbatTimer = 0; uint8_t axis, prop1, prop2; static uint8_t ind = 0; uint16_t vbatRaw = 0; static uint16_t vbatRawArray[8]; uint8_t i; // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value if (rcData[THROTTLE] < BREAKPOINT) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { prop2 = 100 - (uint16_t) cfg.dynThrPID * (rcData[THROTTLE] - BREAKPOINT) / (2000 - BREAKPOINT); } else { prop2 = 100 - cfg.dynThrPID; } } for (axis = 0; axis < 3; axis++) { tmp = min(abs(rcData[axis] - cfg.midrc), 500); if (axis != 2) { // ROLL & PITCH if (cfg.deadband) { if (tmp > cfg.deadband) { tmp -= cfg.deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500; prop1 = (uint16_t) prop1 *prop2 / 100; } else { // YAW if (cfg.yawdeadband) { if (tmp > cfg.yawdeadband) { tmp -= cfg.yawdeadband; } else { tmp = 0; } } rcCommand[axis] = tmp; prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500; } dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100; dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100; if (rcData[axis] < cfg.midrc) rcCommand[axis] = -rcCommand[axis]; } tmp = constrain(rcData[THROTTLE], cfg.mincheck, 2000); tmp = (uint32_t) (tmp - cfg.mincheck) * 1000 / (2000 - cfg.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 = (heading - headFreeModeHold) * M_PI / 180.0f; 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)) { if (!(++vbatTimer % VBATFREQ)) { vbatRawArray[(ind++) % 8] = adcGetBattery(); for (i = 0; i < 8; i++) vbatRaw += vbatRawArray[i]; vbat = batteryAdcToVoltage(vbatRaw / 8); } if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { // VBAT ok, buzzer off buzzerFreq = 0; } else buzzerFreq = 4; // low battery } buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis LED0_TOGGLE; } else { if (f.ACC_CALIBRATED) LED0_OFF; if (f.ARMED) LED0_ON; // This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes. if (feature(FEATURE_TELEMETRY)) initTelemetry(f.ARMED); } #ifdef LEDRING if (feature(FEATURE_LED_RING)) { static uint32_t LEDTime; if ((int32_t)(currentTime - LEDTime) >= 0) { LEDTime = currentTime + 50000; ledringState(); } } #endif if ((int32_t)(currentTime - calibratedAccTime) >= 0) { if (!f.SMALL_ANGLES_25) { f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated LED0_TOGGLE; calibratedAccTime = currentTime + 500000; } else { f.ACC_CALIBRATED = 1; } } serialCom(); if (sensors(SENSOR_GPS)) { static uint32_t GPSLEDTime; if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) { GPSLEDTime = currentTime + 150000; LED1_TOGGLE; } } // 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); else { // TODO MCU temp } }