diff --git a/Src/can.c b/Src/can.c index 19e132b..fe60482 100644 --- a/Src/can.c +++ b/Src/can.c @@ -81,7 +81,7 @@ void MX_CAN2_Init(void) hcan2.Init.AutoWakeUp = DISABLE; hcan2.Init.AutoRetransmission = DISABLE; hcan2.Init.ReceiveFifoLocked = DISABLE; - hcan2.Init.TransmitFifoPriority = DISABLE; + hcan2.Init.TransmitFifoPriority = ENABLE; if (HAL_CAN_Init(&hcan2) != HAL_OK) { Error_Handler(); diff --git a/Src/stm32f4xx_it.c b/Src/stm32f4xx_it.c index 06a3db6..62b6f5c 100644 --- a/Src/stm32f4xx_it.c +++ b/Src/stm32f4xx_it.c @@ -108,11 +108,12 @@ void NMI_Handler(void) void HardFault_Handler(void) { /* USER CODE BEGIN HardFault_IRQn 0 */ - + asm("bx lr"); /* USER CODE END HardFault_IRQn 0 */ while (1) { /* USER CODE BEGIN W1_HardFault_IRQn 0 */ + asm("bx lr"); /* USER CODE END W1_HardFault_IRQn 0 */ } } diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index bf8272f..6ed40d2 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -54,8 +54,8 @@ void GimbalInit() .cali_mode = BMI088_CALIBRATE_ONLINE_MODE, .work_mode = BMI088_BLOCK_PERIODIC_MODE, }; - // imu=BMI088Register(&imu_config); - gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 + imu=BMI088Register(&imu_config); + // gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW Motor_Init_Config_s yaw_config = { .can_init_config = { diff --git a/basic_framework.ioc b/basic_framework.ioc index 6507887..011cd40 100644 --- a/basic_framework.ioc +++ b/basic_framework.ioc @@ -20,8 +20,9 @@ CAN2.BS1=CAN_BS1_10TQ CAN2.BS2=CAN_BS2_3TQ CAN2.CalculateBaudRate=1000000 CAN2.CalculateTimeQuantum=71.42857142857143 -CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,CalculateBaudRate +CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,CalculateBaudRate,TXFP CAN2.Prescaler=3 +CAN2.TXFP=ENABLE Dma.ADC1.8.Direction=DMA_PERIPH_TO_MEMORY Dma.ADC1.8.FIFOMode=DMA_FIFOMODE_DISABLE Dma.ADC1.8.Instance=DMA2_Stream4 diff --git a/modules/BMI088/bmi088.c b/modules/BMI088/bmi088.c index 91ab6c9..1a45fdd 100644 --- a/modules/BMI088/bmi088.c +++ b/modules/BMI088/bmi088.c @@ -151,7 +151,7 @@ static uint8_t BMI088AccelInit(BMI088Instance *bmi088) */ static uint8_t BMI088GyroInit(BMI088Instance *bmi088) { - // 后续添加reset和通信检查 + // 后续添加reset和通信检查? // code to go here ... BMI088GyroWriteSingleReg(bmi088, BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE); // 软复位 DWT_Delay(0.08); @@ -164,9 +164,8 @@ static uint8_t BMI088GyroInit(BMI088Instance *bmi088) DWT_Delay(0.001); // 初始化寄存器,提高可读性 - uint8_t reg = 0; - uint8_t data = 0; - uint8_t error = 0; + uint8_t reg = 0, data = 0; + BMI088_ERORR_CODE_e error = 0; // 使用sizeof而不是magic number,这样如果修改了数组大小,不用修改这里的代码;或者使用宏定义 for (uint8_t i = 0; i < sizeof(BMI088_Gyro_Init_Table) / sizeof(BMI088_Gyro_Init_Table[0]); i++) { @@ -195,25 +194,31 @@ static void BMI088AccSPIFinishCallback(SPIInstance *spi) { static BMI088Instance *bmi088; bmi088 = (BMI088Instance *)(spi->id); - // code to go here ... + // 若第一次读取加速度,则在这里启动温度读取 + // 如果使用异步姿态更新,此处唤醒量测更新的任务 } static void BMI088GyroSPIFinishCallback(SPIInstance *spi) { static BMI088Instance *bmi088; bmi088 = (BMI088Instance *)(spi->id); + // 若不是异步,啥也不做;否则启动姿态的预测步(propagation) } static void BMI088AccINTCallback(GPIOInstance *gpio) { static BMI088Instance *bmi088; bmi088 = (BMI088Instance *)(gpio->id); + // 启动加速度计数据读取(和温度读取,如果有必要),并转换为实际值 + // 读取完毕会调用BMI088AccSPIFinishCallback } static void BMI088GyroINTCallback(GPIOInstance *gpio) { static BMI088Instance *bmi088; bmi088 = (BMI088Instance *)(gpio->id); + // 启动陀螺仪数据读取,并转换为实际值 + // 读取完毕会调用BMI088GyroSPIFinishCallback } // -------------------------以上为私有函数,private用于IT模式下的中断处理---------------------------------// @@ -227,64 +232,44 @@ static void BMI088GyroINTCallback(GPIOInstance *gpio) * @param bmi088 * @return BMI088_Data_t */ -BMI088_Data_t BMI088Acquire(BMI088Instance *bmi088) +uint8_t BMI088Acquire(BMI088Instance *bmi088, BMI088_Data_t *data_store) { - // 分配空间保存返回的数据,指针传递 - static BMI088_Data_t data_store; - static float dt_imu = 1.0; // 初始化为1,这样也可以不用first_read_flag,各有优劣 // 如果是blocking模式,则主动触发一次读取并返回数据 - static uint8_t buf[6] = {0}; // 最多读取6个byte(gyro/acc,temp是2) - static uint8_t first_read_flag; // 判断是否时第一次进入此函数(第一次读取) - // 用于初始化DWT的计数,暂时没想到更好的方法 - if (!first_read_flag) - DWT_GetDeltaT(&bmi088->bias_dwt_cnt); // 初始化delta - else - dt_imu = DWT_GetDeltaT(&bmi088->bias_dwt_cnt); + if (bmi088->work_mode == BMI088_BLOCK_PERIODIC_MODE) + { + static uint8_t buf[6] = {0}; // 最多读取6个byte(gyro/acc,temp是2) + // 读取accel的x轴数据首地址,bmi088内部自增读取地址 // 3* sizeof(int16_t) + BMI088AccelRead(bmi088, BMI088_ACCEL_XOUT_L, buf, 6); + for (uint8_t i = 0; i < 3; i++) + data_store->acc[i] = bmi088->acc_coef * (float)(int16_t)(((buf[2 * i + 1]) << 8) | buf[2 * i]); + BMI088GyroRead(bmi088, BMI088_GYRO_X_L, buf, 6); // 连续读取3个(3*2=6)轴的角速度 + for (uint8_t i = 0; i < 3; i++) + data_store->gyro[i] = bmi088->BMI088_GYRO_SEN * (float)(int16_t)(((buf[2 * i + 1]) << 8) | buf[2 * i]); + BMI088AccelRead(bmi088, BMI088_TEMP_M, buf, 2); // 读温度,温度传感器在accel上 + data_store->temperature = (float)(int16_t)(((buf[0] << 3) | (buf[1] >> 5))) * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET; - // 读取accel的x轴数据首地址,bmi088内部自增读取地址 // 3* sizeof(int16_t) - BMI088AccelRead(bmi088, BMI088_ACCEL_XOUT_L, buf, 6); - static float calc_coef_acc; // 防止重复计算 - if (!first_read_flag) // 初始化的时候赋值 - calc_coef_acc = bmi088->BMI088_ACCEL_SEN * bmi088->acc_coef; // 你要是不爽可以用宏或者全局变量,但我认为你现在很爽 - bmi088->acc[0] = calc_coef_acc * (float)(int16_t)(((buf[1]) << 8) | buf[0]); - bmi088->acc[1] = calc_coef_acc * (float)(int16_t)(((buf[3]) << 8) | buf[2]); - bmi088->acc[3] = calc_coef_acc * (float)(int16_t)(((buf[5]) << 8) | buf[4]); - - BMI088GyroRead(bmi088, BMI088_GYRO_X_L, buf, 6); // 连续读取3个(3*2=6)轴的角速度 - static float gyrosen, bias1, bias2, bias3; - if (!first_read_flag) - { // 先保存,减少访问内存的开销,直接访问栈上变量 - gyrosen = bmi088->BMI088_GYRO_SEN; - bias1 = bmi088->gyro_offset[0]; - bias2 = bmi088->gyro_offset[1]; - bias3 = bmi088->gyro_offset[2]; - - first_read_flag = 1; // 最后在这里,完成一次读取,标志第一次读取完成 - } // 别担心,初始化调用的时候offset(即零飘bias)是0 - bmi088->gyro[0] = (float)(int16_t)(((buf[1]) << 8) | buf[0]) * gyrosen - bias1; - bmi088->gyro[0] = (float)(int16_t)(((buf[3]) << 8) | buf[2]) * gyrosen - bias2; - bmi088->gyro[0] = (float)(int16_t)(((buf[5]) << 8) | buf[4]) * gyrosen - bias3; - - BMI088AccelRead(bmi088, BMI088_TEMP_M, buf, 2); // 读温度,温度传感器在accel上 - bmi088->temperature = (float)(int16_t)(((buf[0] << 3) | (buf[1] >> 5))) * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET; - - return data_store; + return 1; + } // 如果是IT模式,则检查标志位.当传感器数据准备好会触发外部中断,中断服务函数会将标志位置1 if (bmi088->work_mode == BMI088_BLOCK_TRIGGER_MODE && bmi088->update_flag.imu_ready == 1) - return data_store; + { + memcpy(data_store, &bmi088->gyro, sizeof(BMI088_Data_t)); + bmi088->update_flag.imu_ready = 0; + return 1; + } // 如果数据还没准备好,则返回空数据?或者返回上一次的数据?或者返回错误码? @todo if (bmi088->update_flag.imu_ready == 0) - return data_store; + return 0; } /* pre calibrate parameter to go here */ #warning REMEMBER TO SET PRE CALIBRATE PARAMETER IF YOU CHOOSE NOT TO CALIBRATE #define BMI088_PRE_CALI_ACC_X_OFFSET 0.0f #define BMI088_PRE_CALI_ACC_Y_OFFSET 0.0f -// macro to go here... 预设标定参数 gNorm - +#define BMI088_PRE_CALI_ACC_Z_OFFSET 0.0f +#define BMI088_PRE_CALI_G_NORM 9.805f /** * @brief BMI088 acc gyro 标定 * @note 标定后的数据存储在bmi088->bias和gNorm中,用于后续数据消噪和单位转换归一化 @@ -300,80 +285,58 @@ void BMI088CalibrateIMU(BMI088Instance *_bmi088) { if (_bmi088->cali_mode == BMI088_CALIBRATE_ONLINE_MODE) // 性感bmi088在线标定,耗时6s { + _bmi088->acc_coef = BMI088_ACCEL_6G_SEN; // 标定完后要乘以9.805/gNorm + _bmi088->BMI088_GYRO_SEN = BMI088_GYRO_2000_SEN; // 后续改为从initTable中获取 // 一次性参数用完就丢,不用static float startTime; // 开始标定时间,用于确定是否超时 uint16_t CaliTimes = 6000; // 标定次数(6s) - int16_t bmi088_raw_temp; // 临时变量,暂存数据移位拼接后的值 uint8_t buf[6] = {0}; // buffer float gyroMax[3], gyroMin[3]; // 保存标定过程中读取到的数据最大值判断是否满足标定环境 float gNormTemp, gNormMax, gNormMin; // 同上,计算矢量范数(模长) float gyroDiff[3], gNormDiff; // 每个轴的最大角速度跨度及其模长 + BMI088_Data_t raw_data; startTime = DWT_GetTimeline_s(); // 循环继续的条件为标定环境不满足 do // 用do while至少执行一次,省得对上面的参数进行初始化 { // 标定超时,直接使用预标定参数(如果有) - if (DWT_GetTimeline_s() - startTime > 12.5) + if (DWT_GetTimeline_s() - startTime > 12.01) { // 两次都没有成功就切换标定模式,丢给下一个if处理,使用预标定参数 _bmi088->cali_mode = BMI088_LOAD_PRE_CALI_MODE; break; } - DWT_Delay(0.005); + DWT_Delay(0.0005); _bmi088->gNorm = 0; - _bmi088->gyro_offset[0] = 0; - _bmi088->gyro_offset[1] = 0; - _bmi088->gyro_offset[2] = 0; + for (uint8_t i = 0; i < 3; i++) // 重置gNorm和零飘 + _bmi088->gyro_offset[i] = 0; -// @todo : 这里也有获取bmi088数据的操作,后续与BMI088Acquire合并.注意标定时的工作模式是阻塞,且offset和acc_coef要初始化成0和1,标定完成后再设定为标定值 + // @todo : 这里也有获取bmi088数据的操作,后续与BMI088Acquire合并.注意标定时的工作模式是阻塞,且offset和acc_coef要初始化成0和1,标定完成后再设定为标定值 for (uint16_t i = 0; i < CaliTimes; ++i) // 提前计算,优化 { - BMI088AccelRead(_bmi088, BMI088_ACCEL_XOUT_L, buf, 6); // 读取 - bmi088_raw_temp = (int16_t)((buf[1]) << 8) | buf[0]; // 拼接 - _bmi088->acc[0] = bmi088_raw_temp * _bmi088->BMI088_ACCEL_SEN; // 计算真实值 - bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2]; - _bmi088->acc[1] = bmi088_raw_temp * _bmi088->BMI088_ACCEL_SEN; - bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4]; - _bmi088->acc[2] = bmi088_raw_temp * _bmi088->BMI088_ACCEL_SEN; - gNormTemp = sqrtf(_bmi088->acc[0] * _bmi088->acc[0] + - _bmi088->acc[1] * _bmi088->acc[1] + - _bmi088->acc[2] * _bmi088->acc[2]); + BMI088Acquire(_bmi088, &raw_data); + gNormTemp = NormOf3d(raw_data.acc); _bmi088->gNorm += gNormTemp; // 计算范数并累加,最后除以calib times获取单次值 - - BMI088GyroRead(_bmi088, BMI088_GYRO_CHIP_ID, buf, 8); // 可保存提前计算,优化 - bmi088_raw_temp = (int16_t)((buf[1]) << 8) | buf[0]; - _bmi088->gyro[0] = bmi088_raw_temp * _bmi088->BMI088_ACCEL_SEN; - _bmi088->gyro_offset[0] += _bmi088->gyro[0]; - bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2]; - _bmi088->gyro[1] = bmi088_raw_temp * _bmi088->BMI088_ACCEL_SEN; - _bmi088->gyro_offset[1] += _bmi088->gyro[1]; - bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4]; - _bmi088->gyro[2] = bmi088_raw_temp * _bmi088->BMI088_ACCEL_SEN; - _bmi088->gyro_offset[2] += _bmi088->gyro[2]; // 累加当前值,最后除以calib times获得零飘 - // 因为标定时传感器静止,所以采集到的值就是漂移 + for (uint8_t i = 0; i < 3; i++) + _bmi088->gyro_offset[i] += raw_data.gyro[i]; // 因为标定时传感器静止,所以采集到的值就是漂移,累加当前值,最后除以calib times获得零飘 if (i == 0) // 避免未定义的行为(else中) { - gNormMax = gNormTemp; // 初始化成当前的重力加速度模长 - gNormMin = gNormTemp; + gNormMax = gNormMin = gNormTemp; // 初始化成当前的重力加速度模长 for (uint8_t j = 0; j < 3; ++j) { - gyroMax[j] = _bmi088->gyro[j]; - gyroMin[j] = _bmi088->gyro[j]; + gyroMax[j] = raw_data.gyro[j]; + gyroMin[j] = raw_data.gyro[j]; } } else // 更新gNorm的Min Max和gyro的minmax { - if (gNormTemp > gNormMax) - gNormMax = gNormTemp; - if (gNormTemp < gNormMin) - gNormMin = gNormTemp; + gNormMax = gNormMax > gNormTemp ? gNormMax : gNormTemp; + gNormMin = gNormMin < gNormTemp ? gNormMin : gNormTemp; for (uint8_t j = 0; j < 3; ++j) { - if (_bmi088->gyro[j] > gyroMax[j]) // 可以写的更简短,宏? :? - gyroMax[j] = _bmi088->gyro[j]; - if (_bmi088->gyro[j] < gyroMin[j]) - gyroMin[j] = _bmi088->gyro[j]; + gyroMax[j] = gyroMax[j] > _bmi088->gyro[j] ? gyroMax[j] : _bmi088->gyro[j]; + gyroMin[j] = gyroMin[j] < _bmi088->gyro[j] ? gyroMin[j] : _bmi088->gyro[j]; } } @@ -387,15 +350,11 @@ void BMI088CalibrateIMU(BMI088Instance *_bmi088) break; // 超出范围了,重开! remake到while循环,外面还有一层 DWT_Delay(0.0005); // 休息一会再开始下一轮数据获取,IMU准备数据需要时间 } - _bmi088->gNorm /= (float)CaliTimes; // 加速度范数重力 for (uint8_t i = 0; i < 3; ++i) _bmi088->gyro_offset[i] /= (float)CaliTimes; // 三轴零飘 - BMI088AccelRead(_bmi088, BMI088_TEMP_M, buf, 2); - bmi088_raw_temp = (int16_t)((buf[0] << 3) | (buf[1] >> 5)); // 保存标定时的温度,如果已知温度和零飘的关系 // 这里直接存到temperature,可以另外增加BMI088Instance的成员变量TempWhenCalib - _bmi088->temperature = bmi088_raw_temp * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET; - + _bmi088->temperature = raw_data.temperature * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET; // 保存标定时的温度,如果已知温度和零飘的关系 // caliTryOutCount++; 保存已经尝试的标定次数?由你. } while (gNormDiff > 0.5f || fabsf(_bmi088->gNorm - 9.8f) > 0.5f || @@ -410,12 +369,12 @@ void BMI088CalibrateIMU(BMI088Instance *_bmi088) // 离线标定 if (_bmi088->cali_mode == BMI088_LOAD_PRE_CALI_MODE) // 如果标定失败也会进来,直接使用离线数据 { - // 读取标定数据 - // code to go here ... _bmi088->gyro_offset[0] = BMI088_PRE_CALI_ACC_X_OFFSET; - // ... - // acc_coef,gNorm ... + _bmi088->gyro_offset[1] = BMI088_PRE_CALI_ACC_Y_OFFSET; + _bmi088->gyro_offset[2] = BMI088_PRE_CALI_ACC_Z_OFFSET; + _bmi088->gNorm = BMI088_PRE_CALI_G_NORM; } + _bmi088->acc_coef *= 9.805 / _bmi088->gNorm; } // 考虑阻塞模式和非阻塞模式的兼容性,通过条件编译(则需要在编译前修改宏定义)或runtime参数判断 @@ -428,14 +387,12 @@ BMI088Instance *BMI088Register(BMI088_Init_Config_s *config) { // 申请内存 BMI088Instance *bmi088_instance = (BMI088Instance *)zero_malloc(sizeof(BMI088Instance)); - // 从右向左赋值,让bsp instance保存指向bmi088_instance的指针(父指针),便于在底层中断中访问bmi088_instance config->acc_int_config.id = config->gyro_int_config.id = config->spi_acc_config.id = config->spi_gyro_config.id = config->heat_pwm_config.id = bmi088_instance; - // @todo: // 目前只实现了!!!阻塞读取模式!!!.如果需要使用IT模式,则需要修改这里的代码,为spi和gpio注册callback(默认为NULL) // 还需要设置SPI的传输模式为DMA模式或IT模式(默认为blocking) @@ -485,14 +442,9 @@ BMI088Instance *BMI088Register(BMI088_Init_Config_s *config) // 可以增加try out times,超出次数则返回错误 } while (error != 0); - // 尚未标定时先设置为默认值,使得数据拼接和缩放可以正常进行,后续合并到BMI088Acquire()?? - bmi088_instance->acc_coef = 1.0; // 尚未初始化时设定为1,使得BMI088Acquire可以正常使用 - bmi088_instance->BMI088_GYRO_SEN = BMI088_GYRO_2000_SEN; // 后续改为从initTable中获取 - bmi088_instance->BMI088_ACCEL_SEN = BMI088_ACCEL_6G_SEN; // 或使用宏字符串拼接 - // bmi088->gNorm = - - // 标定acc和gyro - BMI088CalibrateIMU(bmi088_instance); + bmi088_instance->work_mode = BMI088_BLOCK_PERIODIC_MODE; // 临时设置为阻塞模式 + BMI088CalibrateIMU(bmi088_instance); // 标定acc和gyro + bmi088_instance->work_mode = config->work_mode; // 恢复工作模式 return bmi088_instance; } diff --git a/modules/BMI088/bmi088.h b/modules/BMI088/bmi088.h index ab3c4be..39bc1f8 100644 --- a/modules/BMI088/bmi088.h +++ b/modules/BMI088/bmi088.h @@ -104,7 +104,7 @@ BMI088Instance *BMI088Register(BMI088_Init_Config_s *config); * @param bmi088 BMI088实例指针 * @return BMI088_Data_t 读取到的数据 */ -BMI088_Data_t BMI088Acquire(BMI088Instance *bmi088); +uint8_t BMI088Acquire(BMI088Instance *bmi088,BMI088_Data_t* data_store); /** * @brief 标定传感器.BMI088在初始化的时候会调用此函数. 提供接口方便标定离线数据 diff --git a/modules/algorithm/user_lib.c b/modules/algorithm/user_lib.c index 5fe5042..8556b39 100644 --- a/modules/algorithm/user_lib.c +++ b/modules/algorithm/user_lib.c @@ -174,6 +174,12 @@ float* Norm3d(float* v) return v; } +// 计算模长 +float NormOf3d(float *v) +{ + return Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); +} + // 三维向量叉乘v1 x v2 void Cross3d(float* v1, float* v2,float* res) { diff --git a/modules/algorithm/user_lib.h b/modules/algorithm/user_lib.h index 0197d2e..3028f46 100644 --- a/modules/algorithm/user_lib.h +++ b/modules/algorithm/user_lib.h @@ -88,7 +88,7 @@ extern uint8_t GlobalDebugMode; #define VAL_MAX(a, b) ((a) > (b) ? (a) : (b)) /** - * @brief 返回一块干净的内?,不过仍然需要强制转?为你需要的类型 + * @brief 返回一块干净的内�?,不过仍然需要强制转�?为你需要的类型 * * @param size 分配大小 * @return void* @@ -116,6 +116,8 @@ int float_rounding(float raw); float* Norm3d(float* v); +float NormOf3d(float* v); + void Cross3d(float* v1, float* v2, float* res); float Dot3d(float* v1, float* v2);