void fc_init()
{
DEBUG(" *** Flight computer init ***\n");
//start values
active_page = config.gui.last_page;
if (active_page >= config.gui.number_of_pages)
active_page = 0;
//reset flight status
fc_reset();
//using fake data
#ifdef FAKE_ENABLE
return;
#endif
//temperature state machine
fc.temp.step = 0;
//init DMA
DMA_PWR_ON;
//init calculators
vario_init();
audio_init();
logger_init();
protocol_init();
wind_init();
gps_init();
if (config.connectivity.use_gps)
gps_start();
bt_init();
if (config.connectivity.use_bt)
bt_module_init();
//VCC to baro, acc/mag gyro + i2c pull-ups
mems_power_on();
//init and test i2c
//HW_REW_1504 have two mems enable pins, both have to be enabled!
//HW_REW_1506 have standalone ldo for mems, hence only one pin is needed
if (!mems_i2c_init())
{
DEBUG("ERROR I2C, Wrong board rev? (%02X)\n", hw_revision);
hw_revision = HW_REW_1504;
eeprom_busy_wait();
eeprom_update_byte(&config_ro.hw_revision, hw_revision);
eeprom_busy_wait();
mems_power_init();
io_init();
mems_power_on();
assert(mems_i2c_init());
}
else
{
if (hw_revision == HW_REW_UNKNOWN)
{
hw_revision = HW_REW_1506;
eeprom_busy_wait();
eeprom_update_byte(&config_ro.hw_revision, hw_revision);
eeprom_busy_wait();
mems_power_init();
io_init();
mems_power_on();
mems_i2c_init();
}
}
if (!mems_i2c_init())
{
DEBUG("I2C error!\nUnable to init flight computer!\n");
return;
}
//Barometer
ms5611.Init(&mems_i2c, MS5611_ADDRESS_CSB_LO);
//Magnetometer + Accelerometer
lsm303d_settings lsm_cfg;
lsm_cfg.enabled = true;
lsm_cfg.accOdr = lsm_acc_1600Hz;
lsm_cfg.accScale = lsm_acc_16g;
lsm_cfg.magOdr = lsm_mag_100Hz;
lsm_cfg.magScale = lsm_mag_4g;
lsm_cfg.magHiRes = true;
lsm_cfg.tempEnable = false;
//Acceleration calculation init
accel_calc_init();
//Magnetic field calculation init
mag_calc_init();
//Gyro
l3gd20_settings l3g_cfg;
l3g_cfg.enabled = true;
l3g_cfg.bw = l3g_50Hz;
l3g_cfg.odr = l3g_760Hz;
l3g_cfg.scale = l3g_2000dps;
//SHT21
sht21_settings sht_cfg;
sht_cfg.rh_enabled = true;
sht_cfg.temp_enabled = true;
//XXX: do self-test?
lsm303d.Init(&mems_i2c, lsm_cfg);
lsm303d.Start();
l3gd20.Init(&mems_i2c, l3g_cfg);
l3gd20.Start();
sht21.Init(&mems_i2c, sht_cfg);
//Measurement timer
FC_MEAS_TIMER_PWR_ON;
fc_meas_timer.Init(FC_MEAS_TIMER, timer_div1024);
fc_meas_timer.SetInterruptPriority(MEDIUM);
fc_meas_timer.EnableInterrupts(timer_overflow | timer_compareA | timer_compareB | timer_compareC);
//tight timing! 1 tick 0.032 ms
//MS pressure conversion 9.040 ms
// temperature conversion 0.600 ms
//MAG read 0.152 ms
//ACC read 1.600 ms
//Gyro read 1.000 ms
fc_meas_timer.SetTop(313); // == 10ms
fc_meas_timer.SetCompare(timer_A, 27); // == 0.78 ms
fc_meas_timer.SetCompare(timer_B, 70); // == 2 ms
fc_meas_timer.SetCompare(timer_C, 200); // == 6 ms
ms5611.StartTemperature();
lsm303d.StartReadMag(); //it takes 152us to transfer
_delay_ms(1);
fc_meas_timer.Start();
DEBUG(" *** FC init done ***\n");
}
void fc_init()
{
DEBUG(" *** Flight computer init ***\n");
//load configuration
cfg_load();
//start values
eeprom_busy_wait();
active_page = eeprom_read_byte(&config.gui.last_page);
fc.epoch_flight_start = 0;
fc.autostart_state = false;
fc.temp_step = 0;
//init calculators
vario_init();
audio_init();
gps_init();
if (fc.use_gps)
gps_start();
bt_init();
// if (fc.use_flage & ENABLE_BT)
// bt_module_init();
//VCC to baro, acc/mag gyro
MEMS_POWER_ON;
GpioSetDirection(IO0, OUTPUT);
GpioWrite(IO0, HIGH);
//init and test i2c
if (!mems_i2c_init())
{
DEBUG("ERROR I2C\n");
led_set(0xFF, 0, 0);
}
//Barometer
ms5611.Init(&mems_i2c, MS5611_ADDRESS_CSB_LO);
//Magnetometer + Accelerometer
lsm303d_settings lsm_cfg;
lsm_cfg.enabled = true;
lsm_cfg.accOdr = lsm_acc_1600Hz;
lsm_cfg.accScale = lsm_acc_16g;
lsm_cfg.magOdr = lsm_mag_100Hz;
lsm_cfg.magScale = lsm_mag_4g;
lsm_cfg.magHiRes = true;
lsm_cfg.tempEnable = false;
//Gyro
l3gd20_settings l3g_cfg;
l3g_cfg.enabled = true;
l3g_cfg.bw = l3g_50Hz;
l3g_cfg.odr = l3g_760Hz;
l3g_cfg.scale = l3g_2000dps;
sht21_settings sht_cfg;
sht_cfg.rh_enabled = true;
sht_cfg.temp_enabled = true;
//XXX: do self-test?
lsm303d.Init(&mems_i2c, lsm_cfg);
lsm303d.Start();
l3gd20.Init(&mems_i2c, l3g_cfg);
l3gd20.Start();
sht21.Init(&mems_i2c, sht_cfg);
//Measurement timer
FC_MEAS_TIMER_PWR_ON;
fc_meas_timer.Init(FC_MEAS_TIMER, timer_div256); //125 == 1ms
fc_meas_timer.SetInterruptPriority(MEDIUM);
fc_meas_timer.EnableInterrupts(timer_overflow | timer_compareA | timer_compareB | timer_compareC);
fc_meas_timer.SetTop(125 * 10); // == 10ms
fc_meas_timer.SetCompare(timer_A, 100); // == 0.64ms
fc_meas_timer.SetCompare(timer_B, 430); // == 2.7ms
fc_meas_timer.SetCompare(timer_C, 555); // == 3.7ms
fc_meas_timer.Start();
DEBUG(" *** FC init done ***\n");
}