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
}
}
