void imuInit(void) { smallAngle = lrintf(acc_1G * cosf(RAD * cfg.small_angle)); accVelScale = 9.80665f / acc_1G / 10000.0f; throttleAngleScale = (1800.0f / M_PI) * (900.0f / cfg.throttle_correction_angle); #ifdef MAG // if mag sensor is enabled, use it if (sensors(SENSOR_MAG)) Mag_init(); #endif }
void imuInit(void) // Initialize & precalculate some values here { if (cfg.gy_smrll || cfg.gy_smptc || cfg.gy_smyw) { SmoothingFactor[ROLL] = cfg.gy_smrll; SmoothingFactor[PITCH] = cfg.gy_smptc; SmoothingFactor[YAW] = cfg.gy_smyw; GyroSmoothing = true; } else GyroSmoothing = false; #ifdef MAG if (sensors(SENSOR_MAG)) Mag_init(); #endif }