/* 功能初始化 */
static void function_init(void)
{
/* imu mpu9250 */
mpu9250_init();
debug_log("mpu9250 初始化完成.\r\n");
/* 参数设置 */
data_config();
debug_log("参数设置完成.\r\n");
/* 融合算法初始化 */
debug_log("融合算法初始化完成.\r\n");
fusion_init();
/* step2: 启动心跳中触发imu模块读取 */
debug_log("imu i2c中断读启动.\r\n");
imu_start(); /* TODO:中断读还是有BUG 性能问题? */
/* 计算一个融合周期(默认10ms) 融合算法需要时间 */
/* 测试融合性能达标 */
debug_log("融合算法测试启动启动.\r\n");
fusion_test_a_fusion_period();
/* step3: 启动任务 */
}
void ICACHE_FLASH_ATTR user_init(void) {
uart_div_modify(0, UART_CLK_FREQ / BAUD);
wifi_set_opmode(STATION_MODE);
wifi_station_connect();
/*** I²C init ***/
i2c_master_gpio_init();
mpu9250_init();
ak8963_init();
os_delay_us(10000);
i2c_scanbus();
/*** LCD Init ***/
ssd1306_init(0);
ssd1306_text_small(10, 10, "Init...");
ssd1306_flip();
// ~Charge input
PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTCK_U, FUNC_GPIO13);
gpio_output_set(0, 0, 0, BIT13);
// Button inputs
buttons_init();
ak8963_calibration_start(10000, on_ak8963_calibration);
os_printf("Startup\r\n");
}
bool initialiseIMU(mpu9250_t *dev)
{
int result;
printf("+------------Initializing------------+\n");
result = mpu9250_init(dev, I2C_0, MPU9250_HW_ADDR_HEX_68, MPU9250_COMP_ADDR_HEX_0C);
if (result == -1) {
puts("[Error] The given i2c is not enabled");
return false;
}
else if (result == -2) {
puts("[Error] The compass did not answer correctly on the given address");
return false;
}
result = mpu9250_set_gyro_fsr(dev, GFSR);
if (result == -1) {
puts("[Error] The given i2c is not enabled");
return false;
}
else if (result == -2) {
puts("[Error] Invalid Gyro FSR value");
return false;
}
result = mpu9250_set_accel_fsr(dev, AFSR);
if (result == -1) {
puts("[Error] The given i2c is not enabled");
return false;
}
else if (result == -2) {
puts("[Error] Invalid Accel FSR value");
return false;
}
mpu9250_set_sample_rate(dev, GA_SAMPLE_RATE_HZ);
uint16_t gaSampleRate = dev->conf.sample_rate;
printf("G+A sample rate set to: %u (requested rate was %u)\n", gaSampleRate, GA_SAMPLE_RATE_HZ);
mpu9250_set_compass_sample_rate(dev, C_SAMPLE_RATE_HZ);
uint16_t cSampleRate = dev->conf.compass_sample_rate;
printf("Compass sample rate set to: %u (requested rate was %u)\n", cSampleRate, C_SAMPLE_RATE_HZ);
printf("Initialization successful\n\n");
t0 = xtimer_now64();
autoCalibrate = true;
dupInterval = UPDATE_INTERVAL_MS;
initialisePosition();
return true;
}
