static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
{
int8_t axis;
static int32_t g[3];
static stdev_t var[3];
for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle()) {
g[axis] = 0;
devClear(&var[axis]);
}
// Sum up CALIBRATING_GYRO_CYCLES readings
g[axis] += gyroADC[axis];
devPush(&var[axis], gyroADC[axis]);
// Reset global variables to prevent other code from using un-calibrated data
gyroADC[axis] = 0;
gyroZero[axis] = 0;
if (isOnFinalGyroCalibrationCycle()) {
float dev = devStandardDeviation(&var[axis]);
// check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
return;
}
gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
blinkLedAndSoundBeeper(10, 15, 1);
}
}
calibratingG--;
}
void changeProfile(uint8_t profileIndex)
{
masterConfig.current_profile_index = profileIndex;
writeEEPROM();
readEEPROM();
blinkLedAndSoundBeeper(2, 40, profileIndex + 1);
}
void mwArm(void)
{
if (f.OK_TO_ARM) {
if (f.ARMED) {
return;
}
if (!f.PREVENT_ARMING) {
f.ARMED = 1;
headFreeModeHold = heading;
return;
}
}
if (!f.ARMED) {
blinkLedAndSoundBeeper(2, 255, 1);
}
}
void mwArm(void)
{
if (ARMING_FLAG(OK_TO_ARM)) {
if (ARMING_FLAG(ARMED)) {
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED);
headFreeModeHold = heading;
return;
}
}
if (!ARMING_FLAG(ARMED)) {
blinkLedAndSoundBeeper(2, 255, 1);
}
}
void mwArm(void)
{
if (ARMING_FLAG(OK_TO_ARM)) {
if (ARMING_FLAG(ARMED)) {
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED);
headFreeModeHold = heading;
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
while (sharedPort) {
mspReleasePortIfAllocated(sharedPort);
sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
}
}
#endif
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
if (sharedBlackboxAndMspPort) {
mspReleasePortIfAllocated(sharedBlackboxAndMspPort);
}
startBlackbox();
}
#endif
disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
return;
}
}
if (!ARMING_FLAG(ARMED)) {
blinkLedAndSoundBeeper(2, 255, 1);
}
}
void readEEPROMAndNotify(void)
{
// re-read written data
readEEPROM();
blinkLedAndSoundBeeper(15, 20, 1);
}
