代码收藏家 STM32 HAL 硬件IIC驱动BNO085（附上源码）
做大创买的BNO085，可后来换了IMU，我就打算拿这个来做飞控得了，反正这么好的IMU不用白不用。
之前问过tb商家要资料，没有stm32的；也找过很多文章，没找到一篇很详细说怎么移植这个IMU的（详细的都要VIP，我就算了，就当作学习分享吧）。可能是这个IMU太贵了吧，不过贵也有它贵的原因，精度高嘛（用mpu6050实在绷不住了，自己用卡尔曼滤波做姿态解算但是缺个磁力计，计算不了Yaw）。
我试过用软件IIC，后来失败了。现在放假，还是不甘心，又想尝试用硬件IIC来做了。没想到这次一晚上就搞定了，果然还是HAL库的硬件IIC强。
本来也想用esp32来驱动的，但我的esp32一用串口打印就会报错（我也不知道为什么，希望看到的大佬可以帮我解释一下），干脆不用esp32了，就用stm32（我也经常用stm32，比较熟悉）。
源码在后面。
代码移植这里的，改一改就可以用了。开源BNO085陀螺仪读取数据 – 全国大学生智能汽车竞赛，清华卓老师 国芯技术交流网站 – AI32位8051交流社区
1. RCC，选外部晶振，时钟频率100MHz（我的是STM32F411CEU6）。
2. SYS里面打开Serial Wire，用于stlink烧录程序（不用管TIM11，我这是打开了FreeRTOS，所以要选择其它时钟）。
3. IIC配置，使用的是IIC1，PB6是SCL，PB7是SDA，选择Fast Mode（手册说支持100k和400k，都用硬件IIC了，用就用最快的）。
4. 打开串口1（调试看数据的）
5. 项目管理，选好存放项目的路径
（这里的IDE要注意一下，因为我用的是vscode而不是Keil，所以选的是Makefile，用Keil的要选MDK-ARM。也想用vscode的话，可以看这个视频，这里的重点不是这个。抛弃keil？VScode开发stm32完整教程_哔哩哔哩_bilibili）
勾上这些
然后生成项目。
6. 下载好开头的文件，将这2个文件复制到自己的项目里面
这个是我的项目。
7. 在main.h里面include一下”bno08x.h“，然后编译。
报错1
改为”#include "main.h"“，再编译。
报错2
删掉它，再编译。
报错3，一大堆，这个不慌。
删掉这个delay。
再划到IIC部分，这些typedef删掉。
保留void I2C_Init(void)，把下面每个IIC的时序删掉，void WriteNbyte(u8 *p, u8 number)和void ReadNbyte(u8 *p, u8 number)里面的时序也删掉，再编译。
这次没报错，但是有个问题，就是delay被我删掉了，所以要把delay改成HAL_Delay()。ctrl+F查找delay，选上替换（replace），填入HAL_Delay。只有4个，全部替换，再编译。
换完之后再编译，没问题，继续改代码。
8. include一下“i2c.h”
9. 修改void I2C_Init(void) 、void WriteNbyte(u8 *p, u8 number)和void ReadNbyte(u8 *p, u8 number)
在头文件先添加这2个地址，这个是BNO085的读和写地址。
再回到c文件，将3个函数改成下面这样。
编译，没毛病。
10. 点开头文件，注释掉这2个有虚线的函数。
11. 回到main里面, 初始化一下IIC
12. 复位一下，然后这些enable全都用上一下，全部填100（这个是IMU发数据的时间间隔，以ms为单位），开一下校准
可以只选自己想要的数据，这里我懒得选了，全都用上。可以参考手册说的频率来设置。
13. 读一下加速度的数据，大循环写下面的代码，编译烧录。
（这里要先重定向一下串口
这个是Keil的
【STM32】HAL库三步实现串口重定向（代码复制可用）_stm32串口重定向hal库-CSDN博客
这个是vscode的
基于 VsCode + GCC + STM32 环境下的串口输入输出重定向_gcc libc 重定向-CSDN博客）
14. 打开VOFA+看一下数据
没有数据，这里也别慌，这个是用vscode打印浮点数的毛病，
打开”构建配置：GCC“的“构建器选型”，在“编译器附加选项”添加“ -u_printf_float”
修改后（一定要点一下“全部保存”，不然不会自动保存）
之后再编译烧录。
可以打印了，但是数据好像有点问题。
有255的，这说明数据类型转换出了问题，跳到getAccelX()的定义
再跳到qToFloat的定义
这里看起来好像是qPoint的类型有问题，我改过之后，还是不行。后来我想到，int类型在不同的单片机或者不同的系统代表的数据类型不一样，可能是int16_t也可能是int32_t，所以我打印了一下fixedPointValue。先注释掉大循环里面的打印，在这里面打印一下fixedPointValue看看。
发现有65531这种接近65535的数据，说明确实是我的单片机的int代表的是int32_t类型，将fixedPointValue的类型改为int16_t，头文件的声明也要改一下，编译烧录。
这个看起来正常多了。去大循环打印一下加速度看看。
现在就正常了。
15. 读一下四元数，解算一下姿态角。第1个旋转向量就是四元数，只需要打开第1个enable，四元数转欧拉角的公式问一下GPT就可以了，挺简单的。这里要注意的是要单位转换一下，因为atan2和asin返回的数据是以rad为单位的，要乘一下57.3（就是180/pi）。编译烧录。
可以看到解算到的姿态角，这个姿态角的yaw可能是静止参考系的，每次复位都是有一个角度。
下面是源码（我一开始开了FreeRTOS，我把调度器给注释了才能到大循环的，这里要注意一下）：
mian.c
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2024 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "cmsis_os.h"
#include "dma.h"
#include "i2c.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
__IO uint8_t InitOK = 0;
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
void MX_FREERTOS_Init(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
MX_TIM4_Init();
MX_SPI1_Init();
MX_USART2_UART_Init();
MX_TIM10_Init();
MX_TIM2_Init();
MX_TIM3_Init();
MX_USART1_UART_Init();
MX_USART6_UART_Init();
MX_I2C1_Init();
/* USER CODE BEGIN 2 */
OLED_Init( OLED_ADDR_B );
I2C_Init(); // BNO085 IIC 初始化
uint8_t data;
HAL_UART_Receive_IT(&huart2, &data, 1);
printf("Init OK!!!\r\n");
/* USER CODE END 2 */
/* Call init function for freertos objects (in cmsis_os2.c) */
// MX_FREERTOS_Init();
/* Start scheduler */
// osKernelStart();
/* We should never get here as control is now taken by the scheduler */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
float q0, q1, q2, q3;
float roll, pitch, yaw;
// 重置传感器
softReset();
enableRotationVector(1);
// enableGameRotationVector(100);
// enableAccelerometer(100);
// enableLinearAccelerometer(100);
// enableGyro(100);
// enableMagnetometer(100);
// enableStepCounter(100);
// enableStabilityClassifier(100);
// 开始校准（可选）
calibrateAll();
while (1)
{
if ( dataAvailable() )
{
q0 = getQuatReal();
q1 = getQuatI();
q2 = getQuatJ();
q3 = getQuatK();
roll = atan2( 2 * ( q0 * q1 + q2 * q3 ) , 1- 2 * ( q1 * q1 + q2 * q2 ) ) * 57.3;
pitch = asin( 2 * ( q0 * q2 - q3 * q1 ) ) * 57.3;
yaw = atan2( 2 * ( q0 * q3 + q1 * q2 ) , 1 - 2 * ( q2 * q2 + q3 * q3 ) ) * 57.3;
printf("%f,%f,%f\n", roll, pitch, yaw);
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Configure the main internal regulator output voltage
*/
__HAL_RCC_PWR_CLK_ENABLE();
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLM = 12;
RCC_OscInitStruct.PLL.PLLN = 96;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = 4;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK)
{
Error_Handler();
}
}
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */
/**
* @brief Period elapsed callback in non blocking mode
* @note This function is called when TIM11 interrupt took place, inside
* HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
* a global variable "uwTick" used as application time base.
* @param htim : TIM handle
* @retval None
*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
/* USER CODE BEGIN Callback 0 */
/* USER CODE END Callback 0 */
if (htim->Instance == TIM11) {
HAL_IncTick();
}
/* USER CODE BEGIN Callback 1 */
if (htim->Instance == TIM4) {
ulFreeRTOSRunTimeTicks++;
}
/* USER CODE END Callback 1 */
}
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
bno08x.c
#include "bno08x.h"
#include "math.h"
#include "i2c.h"
// Global Variables
unsigned char shtpHeader[4]; // Each packet has a header of 4 chars
unsigned char shtpData[MAX_PACKET_SIZE];
unsigned char sequenceNumber[6] = {0, 0, 0, 0, 0, 0}; // There are 6 com channels. Each channel has its own seqnum
unsigned char commandSequenceNumber = 0; // Commands have a seqNum as well. These are inside command packet, the header uses its own seqNum per channel
unsigned long metaData[MAX_METADATA_SIZE]; // There is more than 10 words in a metadata record but we'll stop at Q point 3
// Variables
unsigned char _deviceAddress;//跟踪 I2C 地址。setI2CAddress 更改了这一点。
// These are the raw sensor values pulled from the user requested Input Report
unsigned int rawAccelX, rawAccelY, rawAccelZ, accelAccuracy;
unsigned int rawLinAccelX, rawLinAccelY, rawLinAccelZ, accelLinAccuracy;
unsigned int rawGyroX, rawGyroY, rawGyroZ, gyroAccuracy;
unsigned int rawMagX, rawMagY, rawMagZ, magAccuracy;
unsigned int rawQuatI, rawQuatJ, rawQuatK, rawQuatReal, rawQuatRadianAccuracy, quatAccuracy;
unsigned int stepCount;
unsigned char stabilityClassifier;
unsigned char activityClassifier;
unsigned char *_activityConfidences; // Array that store the confidences of the 9 possible activities
// These Q values are defined in the datasheet but can also be obtained by querying the meta data records
// See the read metadata example for more info
int rotationVector_Q1 = 14;
int accelerometer_Q1 = 8;
int linear_accelerometer_Q1 = 8;
int gyro_Q1 = 9;
int magnetometer_Q1 = 4;
char dataAvailable(void)
{
if (receivePacket() == 1)
{
// Check to see if this packet is a sensor reporting its data to us
if (shtpHeader[2] == CHANNEL_REPORTS && shtpData[0] == SHTP_REPORT_BASE_TIMESTAMP)
{
parseInputReport(); // This will update the rawAccelX, etc variables depending on which feature report is found
return (1);
}
}
return (0);
}
// This function pulls the data from the input report
// The input reports vary in length so this function stores the various 16-bit values as globals
// Unit responds with packet that contains the following:
// shtpHeader[0:3]: First, a 4 byte header
// shtpData[0:4]: Then a 5 byte timestamp of microsecond clicks since reading was taken
// shtpData[5 + 0]: Then a feature report ID (0x01 for Accel, 0x05 for Rotation Vector)
// shtpData[5 + 1]: Sequence number (See 6.5.18.2)
// shtpData[5 + 2]: Status
// shtpData[3]: HAL_Delay
// shtpData[4:5]: i/accel x/gyro x/etc
// shtpData[6:7]: j/accel y/gyro y/etc
// shtpData[8:9]: k/accel z/gyro z/etc
// shtpData[10:11]: real/gyro temp/etc
// shtpData[12:13]: Accuracy estimate
void parseInputReport(void)
{
// Calculate the number of data bytes in this packet
int dataLength = ((unsigned int)shtpHeader[1] << 8 | shtpHeader[0]);
unsigned char status = shtpData[5 + 2] & 0x03; // Get status bits
unsigned int data1 = (unsigned int)shtpData[5 + 5] << 8 | shtpData[5 + 4];
unsigned int data2 = (unsigned int)shtpData[5 + 7] << 8 | shtpData[5 + 6];
unsigned int data3 = (unsigned int)shtpData[5 + 9] << 8 | shtpData[5 + 8];
unsigned int data4 = 0;
unsigned int data5 = 0;
unsigned char x = 0;
dataLength &= ~(1 << 15); // Clear the MSbit. This bit indicates if this package is a continuation of the last.
// Ignore it for now. TODO catch this as an error and exit
dataLength -= 4; // Remove the header bytes from the data count
if (dataLength - 5 > 9)
{
data4 = (unsigned int)shtpData[5 + 11] << 8 | shtpData[5 + 10];
}
if (dataLength - 5 > 11)
{
data5 = (unsigned int)shtpData[5 + 13] << 8 | shtpData[5 + 12];
}
// Store these generic values to their proper global variable
if (shtpData[5] == SENSOR_REPORTID_ACCELEROMETER)
{
accelAccuracy = status;
rawAccelX = data1;
rawAccelY = data2;
rawAccelZ = data3;
}
else if (shtpData[5] == SENSOR_REPORTID_LINEAR_ACCELERATION)
{
accelLinAccuracy = status;
rawLinAccelX = data1;
rawLinAccelY = data2;
rawLinAccelZ = data3;
}
else if (shtpData[5] == SENSOR_REPORTID_GYROSCOPE)
{
gyroAccuracy = status;
rawGyroX = data1;
rawGyroY = data2;
rawGyroZ = data3;
}
else if (shtpData[5] == SENSOR_REPORTID_MAGNETIC_FIELD)
{
magAccuracy = status;
rawMagX = data1;
rawMagY = data2;
rawMagZ = data3;
}
else if (shtpData[5] == SENSOR_REPORTID_ROTATION_VECTOR || shtpData[5] == SENSOR_REPORTID_GAME_ROTATION_VECTOR)
{
quatAccuracy = status;
rawQuatI = data1;
rawQuatJ = data2;
rawQuatK = data3;
rawQuatReal = data4;
rawQuatRadianAccuracy = data5; // Only available on rotation vector, not game rot vector
}
else if (shtpData[5] == SENSOR_REPORTID_STEP_COUNTER)
{
stepCount = data3; // Bytes 8/9
}
else if (shtpData[5] == SENSOR_REPORTID_STABILITY_CLASSIFIER)
{
stabilityClassifier = shtpData[5 + 4]; // Byte 4 only
}
else if (shtpData[5] == SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER)
{
activityClassifier = shtpData[5 + 5]; // Most likely state
// Load activity classification confidences into the array
for (x = 0; x < 9; x++) // Hardcoded to max of 9. TODO - bring in array size
_activityConfidences[x] = shtpData[5 + 6 + x]; // 5 bytes of timestamp, byte 6 is first confidence byte
}
else
{
// This sensor report ID is unhandled.
// See reference manual to add additional feature reports as needed
}
// TODO additional feature reports may be strung together. Parse them all.
}
// Return the rotation vector quaternion I
float getQuatI()
{
float quat = qToFloat(rawQuatI, (unsigned char)rotationVector_Q1);
return (quat);
}
// Return the rotation vector quaternion J
float getQuatJ()
{
float quat = qToFloat(rawQuatJ, (unsigned char)rotationVector_Q1);
return (quat);
}
// Return the rotation vector quaternion K
float getQuatK()
{
float quat = qToFloat(rawQuatK, (unsigned char)rotationVector_Q1);
return (quat);
}
// Return the rotation vector quaternion Real
float getQuatReal()
{
float quat = qToFloat(rawQuatReal, (unsigned char)rotationVector_Q1);
return (quat);
}
// Return the rotation vector accuracy
float getQuatRadianAccuracy()
{
float quat = qToFloat(rawQuatRadianAccuracy, (unsigned char)rotationVector_Q1);
return (quat);
}
// Return the acceleration component
unsigned char getQuatAccuracy()
{
return (quatAccuracy);
}
// Return the acceleration component
float getAccelX()
{
float accel = qToFloat(rawAccelX, (unsigned char)accelerometer_Q1);
return (accel);
}
// Return the acceleration component
float getAccelY()
{
float accel = qToFloat(rawAccelY, (unsigned char)accelerometer_Q1);
return (accel);
}
// Return the acceleration component
float getAccelZ()
{
float accel = qToFloat(rawAccelZ, (unsigned char)accelerometer_Q1);
return (accel);
}
// Return the acceleration component
unsigned char getAccelAccuracy()
{
return (accelAccuracy);
}
// linear acceleration, i.e. minus gravity
// Return the acceleration component
float getLinAccelX()
{
float accel = qToFloat(rawLinAccelX, (unsigned char)linear_accelerometer_Q1);
return (accel);
}
// Return the acceleration component
float getLinAccelY()
{
float accel = qToFloat(rawLinAccelY, (unsigned char)linear_accelerometer_Q1);
return (accel);
}
// Return the acceleration component
float getLinAccelZ()
{
float accel = qToFloat(rawLinAccelZ, (unsigned char)linear_accelerometer_Q1);
return (accel);
}
// Return the acceleration component
unsigned char getLinAccelAccuracy()
{
return (accelLinAccuracy);
}
// Return the gyro component
float getGyroX()
{
float gyro = qToFloat(rawGyroX, (unsigned char)gyro_Q1);
return (gyro);
}
// Return the gyro component
float getGyroY()
{
float gyro = qToFloat(rawGyroY, (unsigned char)gyro_Q1);
return (gyro);
}
// Return the gyro component
float getGyroZ()
{
float gyro = qToFloat(rawGyroZ, (unsigned char)gyro_Q1);
return (gyro);
}
// Return the gyro component
unsigned char getGyroAccuracy()
{
return (gyroAccuracy);
}
// Return the magnetometer component
float getMagX()
{
float mag = qToFloat(rawMagX, (unsigned char)magnetometer_Q1);
return (mag);
}
// Return the magnetometer component
float getMagY()
{
float mag = qToFloat(rawMagY, (unsigned char)magnetometer_Q1);
return (mag);
}
// Return the magnetometer component
float getMagZ()
{
float mag = qToFloat(rawMagZ, (unsigned char)magnetometer_Q1);
return (mag);
}
// Return the mag component
unsigned char getMagAccuracy()
{
return (magAccuracy);
}
// Return the step count
unsigned int getStepCount()
{
return (stepCount);
}
// Return the stability classifier
unsigned char getStabilityClassifier()
{
return (stabilityClassifier);
}
// Return the activity classifier
unsigned char getActivityClassifier()
{
return (activityClassifier);
}
// Given a record ID, read the Q1 value from the metaData record in the FRS (ya, it's complicated)
// Q1 is used for all sensor data calculations
int getQ1(unsigned int recordID)
{
// Q1 is always the lower 16 bits of word 7
unsigned int q = readFRSword(recordID, 7) & 0xFFFF; // Get word 7, lower 16 bits
return (q);
}
// Given a record ID, read the Q2 value from the metaData record in the FRS
// Q2 is used in sensor bias
int getQ2(unsigned int recordID)
{
// Q2 is always the upper 16 bits of word 7
unsigned int q = readFRSword(recordID, 7) >> 16; // Get word 7, upper 16 bits
return (q);
}
// Given a record ID, read the Q3 value from the metaData record in the FRS
// Q3 is used in sensor change sensitivity
int getQ3(unsigned int recordID)
{
// Q3 is always the upper 16 bits of word 8
unsigned int q = readFRSword(recordID, 8) >> 16; // Get word 8, upper 16 bits
return (q);
}
// Given a record ID, read the resolution value from the metaData record in the FRS for a given sensor
float getResolution(unsigned int recordID)
{
// The resolution Q value are 'the same as those used in the sensor's input report'
// This should be Q1.
int Q = getQ1(recordID);
// Resolution is always word 2
unsigned long value = readFRSword(recordID, 2); // Get word 2
float resolution = qToFloat((unsigned int)value, (unsigned char)Q);
return (resolution);
}
// Given a record ID, read the range value from the metaData record in the FRS for a given sensor
float getRange(unsigned int recordID)
{
// The resolution Q value are 'the same as those used in the sensor's input report'
// This should be Q1.
int Q = getQ1(recordID);
// Range is always word 1
unsigned long value = readFRSword(recordID, 1); // Get word 1
float range = qToFloat((unsigned int)value, (unsigned char)Q);
return (range);
}
// Given a record ID and a word number, look up the word data
// Helpful for pulling out a Q value, range, etc.
// Use readFRSdata for pulling out multi-word objects for a sensor (Vendor data for example)
unsigned long readFRSword(unsigned int recordID, unsigned char wordNumber)
{
if (readFRSdata(recordID, wordNumber, 1) == 1) // Get word number, just one word in length from FRS
return (metaData[0]); // Return this one word
return (0); // Error
}
// Ask the sensor for data from the Flash Record System
// See 6.3.6 page 40, FRS Read Request
void frsReadRequest(unsigned int recordID, unsigned int readOffset, unsigned int blockSize)
{
shtpData[0] = SHTP_REPORT_FRS_READ_REQUEST; // FRS Read Request
shtpData[1] = 0; // Reserved
shtpData[2] = (readOffset >> 0) & 0xFF; // Read Offset LSB
shtpData[3] = (readOffset >> 8) & 0xFF; // Read Offset MSB
shtpData[4] = (recordID >> 0) & 0xFF; // FRS Type LSB
shtpData[5] = (recordID >> 8) & 0xFF; // FRS Type MSB
shtpData[6] = (blockSize >> 0) & 0xFF; // Block size LSB
shtpData[7] = (blockSize >> 8) & 0xFF; // Block size MSB
// Transmit packet on channel 2, 8 bytes
sendPacket(CHANNEL_CONTROL, 8);
}
// Given a sensor or record ID, and a given start/stop bytes, read the data from the Flash Record System (FRS) for this sensor
// Returns 1 if metaData array is loaded successfully
// Returns 0 if failure
char readFRSdata(unsigned int recordID, unsigned char startLocation, unsigned char wordsToRead)
{
unsigned char spot = 0;
unsigned char dataLength = 0;
unsigned char frsStatus = 0;
unsigned long data0 = 0;
unsigned long data1 = 0;
// First we send a Flash Record System (FRS) request
frsReadRequest(recordID, startLocation, wordsToRead); // From startLocation of record, read a # of words
// Read bytes until FRS reports that the read is complete
while (1)
{
// Now we wait for response
while (1)
{
unsigned char counter = 0;
while (receivePacket() == 0)
{
if (counter++ > 100)
return (0); // Give up
HAL_Delay(1);
}
// We have the packet, inspect it for the right contents
// See page 40. Report ID should be 0xF3 and the FRS types should match the thing we requested
if (shtpData[0] == SHTP_REPORT_FRS_READ_RESPONSE)
if (((unsigned int)shtpData[13] << 8 | shtpData[12]) == recordID)
break; // This packet is one we are looking for
}
dataLength = shtpData[1] >> 4;
frsStatus = shtpData[1] & 0x0F;
data0 = (unsigned long)shtpData[7] << 24 | (unsigned long)shtpData[6] << 16 | (unsigned long)shtpData[5] << 8 | (unsigned long)shtpData[4];
data1 = (unsigned long)shtpData[11] << 24 | (unsigned long)shtpData[10] << 16 | (unsigned long)shtpData[9] << 8 | (unsigned long)shtpData[8];
// Record these words to the metaData array
if (dataLength > 0)
{
metaData[spot++] = data0;
}
if (dataLength > 1)
{
metaData[spot++] = data1;
}
if (frsStatus == 3 || frsStatus == 6 || frsStatus == 7)
{
return (1); // FRS status is read completed! We're done!
}
}
}
// Send command to reset IC
// Read all advertisement packets from sensor
// The sensor has been seen to reset twice if we attempt too much too quickly.
// This seems to work reliably.
void softReset(void)
{
shtpData[0] = 1; // Reset
// Attempt to start communication with sensor
sendPacket(CHANNEL_EXECUTABLE, 1); // Transmit packet on channel 1, 1 byte
// Read all incoming data and flush it
HAL_Delay(50);
while (receivePacket() == 1)
;
HAL_Delay(50);
while (receivePacket() == 1)
;
}
// Get the reason for the last reset
// 1 = POR, 2 = Internal reset, 3 = Watchdog, 4 = External reset, 5 = Other
unsigned char resetReason()
{
shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; // Request the product ID and reset info
shtpData[1] = 0; // Reserved
// Transmit packet on channel 2, 2 bytes
sendPacket(CHANNEL_CONTROL, 2);
// Now we wait for response
if (receivePacket() == 1)
{
if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE)
{
return (shtpData[1]);
}
}
return (0);
}
// Given a register value and a Q point, convert to float
float qToFloat(int16_t fixedPointValue, unsigned char qPoint)
{
float qFloat = fixedPointValue;
qFloat *= pow(2, qPoint * -1);
return (qFloat);
}
// Sends the packet to enable the rotation vector
void enableRotationVector(unsigned int timeBetweenReports)
{
setFeatureCommand(SENSOR_REPORTID_ROTATION_VECTOR, timeBetweenReports, 0);
}
// Sends the packet to enable the rotation vector
void enableGameRotationVector(unsigned int timeBetweenReports)
{
setFeatureCommand(SENSOR_REPORTID_GAME_ROTATION_VECTOR, timeBetweenReports, 0);
}
// Sends the packet to enable the accelerometer
void enableAccelerometer(unsigned int timeBetweenReports)
{
setFeatureCommand(SENSOR_REPORTID_ACCELEROMETER, timeBetweenReports, 0);
}
// Sends the packet to enable the accelerometer
void enableLinearAccelerometer(unsigned int timeBetweenReports)
{
setFeatureCommand(SENSOR_REPORTID_LINEAR_ACCELERATION, timeBetweenReports, 0);
}
// Sends the packet to enable the gyro
void enableGyro(unsigned int timeBetweenReports)
{
setFeatureCommand(SENSOR_REPORTID_GYROSCOPE, timeBetweenReports, 0);
}
// Sends the packet to enable the magnetometer
void enableMagnetometer(unsigned int timeBetweenReports)
{
setFeatureCommand(SENSOR_REPORTID_MAGNETIC_FIELD, timeBetweenReports, 0);
}
// Sends the packet to enable the step counter
void enableStepCounter(unsigned int timeBetweenReports)
{
setFeatureCommand(SENSOR_REPORTID_STEP_COUNTER, timeBetweenReports, 0);
}
// Sends the packet to enable the Stability Classifier
void enableStabilityClassifier(unsigned int timeBetweenReports)
{
setFeatureCommand(SENSOR_REPORTID_STABILITY_CLASSIFIER, timeBetweenReports, 0);
}
// Sends the commands to begin calibration of the accelerometer
void calibrateAccelerometer()
{
sendCalibrateCommand(CALIBRATE_ACCEL);
}
// Sends the commands to begin calibration of the gyro
void calibrateGyro()
{
sendCalibrateCommand(CALIBRATE_GYRO);
}
// Sends the commands to begin calibration of the magnetometer
void calibrateMagnetometer()
{
sendCalibrateCommand(CALIBRATE_MAG);
}
// Sends the commands to begin calibration of the planar accelerometer
void calibratePlanarAccelerometer()
{
sendCalibrateCommand(CALIBRATE_PLANAR_ACCEL);
}
// See 2.2 of the Calibration Procedure document 1000-4044
void calibrateAll()
{
sendCalibrateCommand(CALIBRATE_ACCEL_GYRO_MAG);
}
void endCalibration()
{
sendCalibrateCommand(CALIBRATE_STOP); // Disables all calibrations
}
// Given a sensor's report ID, this tells the BNO080 to begin reporting the values
// Also sets the specific config word. Useful for personal activity classifier
void setFeatureCommand(unsigned char reportID, unsigned int timeBetweenReports, unsigned long specificConfig)
{
long microsBetweenReports = (long)timeBetweenReports * 1000L;
shtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; // Set feature command. Reference page 55
shtpData[1] = reportID; // Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
shtpData[2] = 0; // Feature flags
shtpData[3] = 0; // Change sensitivity (LSB)
shtpData[4] = 0; // Change sensitivity (MSB)
shtpData[5] = (microsBetweenReports >> 0) & 0xFF; // Report interval (LSB) in microseconds. 0x7A120 = 500ms
shtpData[6] = (microsBetweenReports >> 8) & 0xFF; // Report interval
shtpData[7] = (microsBetweenReports >> 16) & 0xFF; // Report interval
shtpData[8] = (microsBetweenReports >> 24) & 0xFF; // Report interval (MSB)
shtpData[9] = 0; // Batch Interval (LSB)
shtpData[10] = 0; // Batch Interval
shtpData[11] = 0; // Batch Interval
shtpData[12] = 0; // Batch Interval (MSB)
shtpData[13] = (specificConfig >> 0) & 0xFF; // Sensor-specific config (LSB)
shtpData[14] = (specificConfig >> 8) & 0xFF; // Sensor-specific config
shtpData[15] = (specificConfig >> 16) & 0xFF; // Sensor-specific config
shtpData[16] = (specificConfig >> 24) & 0xFF; // Sensor-specific config (MSB)
// Transmit packet on channel 2, 17 bytes
sendPacket(CHANNEL_CONTROL, 17);
}
// Tell the sensor to do a command
// See 6.3.8 page 41, Command request
// The caller is expected to set P0 through P8 prior to calling
void sendCommand(unsigned char command)
{
shtpData[0] = SHTP_REPORT_COMMAND_REQUEST; // Command Request
shtpData[1] = commandSequenceNumber++; // Increments automatically each function call
shtpData[2] = command; // Command
// Caller must set these
/*shtpData[3] = 0; //P0
shtpData[4] = 0; //P1
shtpData[5] = 0; //P2
shtpData[6] = 0;
shtpData[7] = 0;
shtpData[8] = 0;
shtpData[9] = 0;
shtpData[10] = 0;
shtpData[11] = 0;*/
// Transmit packet on channel 2, 12 bytes
sendPacket(CHANNEL_CONTROL, 12);
}
// This tells the BNO080 to begin calibrating
// See page 50 of reference manual and the 1000-4044 calibration doc
void sendCalibrateCommand(unsigned char thingToCalibrate)
{
unsigned char x = 0;
/*shtpData[3] = 0; //P0 - Accel Cal Enable
shtpData[4] = 0; //P1 - Gyro Cal Enable
shtpData[5] = 0; //P2 - Mag Cal Enable
shtpData[6] = 0; //P3 - Subcommand 0x00
shtpData[7] = 0; //P4 - Planar Accel Cal Enable
shtpData[8] = 0; //P5 - Reserved
shtpData[9] = 0; //P6 - Reserved
shtpData[10] = 0; //P7 - Reserved
shtpData[11] = 0; //P8 - Reserved*/
for (x = 3; x < 12; x++) // Clear this section of the shtpData array
shtpData[x] = 0;
if (thingToCalibrate == CALIBRATE_ACCEL)
shtpData[3] = 1;
else if (thingToCalibrate == CALIBRATE_GYRO)
shtpData[4] = 1;
else if (thingToCalibrate == CALIBRATE_MAG)
shtpData[5] = 1;
else if (thingToCalibrate == CALIBRATE_PLANAR_ACCEL)
shtpData[7] = 1;
else if (thingToCalibrate == CALIBRATE_ACCEL_GYRO_MAG)
{
shtpData[3] = 1;
shtpData[4] = 1;
shtpData[5] = 1;
}
else if (thingToCalibrate == CALIBRATE_STOP)
; // Do nothing, bytes are set to zero
// Using this shtpData packet, send a command
sendCommand(COMMAND_ME_CALIBRATE);
}
// This tells the BNO080 to save the Dynamic Calibration Data (DCD) to flash
// See page 49 of reference manual and the 1000-4044 calibration doc
void saveCalibration()
{
unsigned char x = 0;
/*shtpData[3] = 0; //P0 - Reserved
shtpData[4] = 0; //P1 - Reserved
shtpData[5] = 0; //P2 - Reserved
shtpData[6] = 0; //P3 - Reserved
shtpData[7] = 0; //P4 - Reserved
shtpData[8] = 0; //P5 - Reserved
shtpData[9] = 0; //P6 - Reserved
shtpData[10] = 0; //P7 - Reserved
shtpData[11] = 0; //P8 - Reserved*/
for (x = 3; x < 12; x++) // Clear this section of the shtpData array
shtpData[x] = 0;
// Using this shtpData packet, send a command
sendCommand(COMMAND_DCD); // Save DCD command
}
// IIC 硬件底层
void I2C_Init(void)
{
MX_I2C1_Init();
}
void WriteNbyte(u8 *p, u8 number) /* WordAddress,First Data Address,Byte lenth */
{
HAL_I2C_Master_Transmit( &hi2c1, BNO080_W_ADDRESS, p, number, 0xffff );
}
void ReadNbyte(u8 *p, u8 number) /* WordAddress,First Data Address,Byte lenth */
{
HAL_I2C_Master_Receive( &hi2c1, BNO080_R_ADDRESS, p, number, 0xffff );
}
u8 i2c_tmp[32] = {0};
// 检查是否有任何可用的新数据
// 将传入数据包的内容读入 shtpData 数组
char receivePacket(void)
{
int dataLength = 0;
unsigned char packetLSB = 0;
unsigned char packetMSB = 0;
unsigned char channelNumber = 0;
unsigned char sequenceNumber = 0;
ReadNbyte(i2c_tmp, 4);
// Get the first four bytes, aka the packet header
packetLSB = i2c_tmp[0];
packetMSB = i2c_tmp[1];
channelNumber = i2c_tmp[2];
sequenceNumber = i2c_tmp[3]; // Not sure if we need to store this or not
// Store the header info.
shtpHeader[0] = packetLSB;
shtpHeader[1] = packetMSB;
shtpHeader[2] = channelNumber;
shtpHeader[3] = sequenceNumber;
// Calculate the number of data bytes in this packet
dataLength = ((unsigned int)packetMSB << 8 | packetLSB);
dataLength &= ~(1 << 15);//清除 MSbit。
//此位指示此包是否是上一个包的延续。暂时忽略它。
//TODO 将此捕获为错误并退出
if (dataLength == 0)
{
// Packet is empty
return (0); // All done
}
dataLength -= 4; // Remove the header bytes from the data count
getData(dataLength);
return (1); // We're done!
}
// 向传感器发送多个请求，直到从传感器接收到所有数据字节
// shtpData 缓冲区的最大容量为 MAX_PACKET_SIZE。超过此量的任何字节都将丢失。
// Arduino I2C 读取限制为 32 字节。标头是 4 个字节，因此我们每个交互可以读取的最大数据为 28 字节
char getData(unsigned int bytesRemaining)
{
unsigned int dataSpot = 0; // Start at the beginning of shtpData array
unsigned char x = 0;
unsigned char incoming = 0;
// Setup a series of chunked 32 byte reads
while (bytesRemaining > 0)
{
unsigned int numberOfBytesToRead = bytesRemaining;
if (numberOfBytesToRead > (I2C_BUFFER_LENGTH - 4))
numberOfBytesToRead = (I2C_BUFFER_LENGTH - 4);
ReadNbyte(i2c_tmp, (unsigned char)(numberOfBytesToRead + 4));
// 前四个字节是标头字节，被丢弃
for (x = 0; x < numberOfBytesToRead; x++)
{
incoming = i2c_tmp[x + 4];
if (dataSpot < MAX_PACKET_SIZE)
{
shtpData[dataSpot++] = incoming; // Store data into the shtpData array
}
else
{
// Do nothing with the data
}
}
bytesRemaining -= numberOfBytesToRead;
}
return (1); // Done!
}
// 给定数据包，发送标头，然后发送数据
// 如果传感器不 ACK，则返回 0
// TODO - Arduino 的最大发送量为 32 字节。如果需要，将发送分解为多个数据包。
char sendPacket(unsigned char channelNumber, unsigned char dataLength)
{
unsigned char packetLength = dataLength + 4; // Add four bytes for the header
// if(packetLength > I2C_BUFFER_LENGTH) return(0); //You are trying to send too much. Break into smaller packets.
unsigned char i = 0;
// Send the 4 byte packet header
i2c_tmp[0] = (packetLength & 0xFF); // Packet length LSB
i2c_tmp[1] = (0); // Packet length MSB
i2c_tmp[2] = (channelNumber); // Channel number
i2c_tmp[3] = (sequenceNumber[channelNumber]++); // Send the sequence number, increments with each packet sent, different counter for each channel
// Send the user's data packet
for (i = 0; i < dataLength; i++)
{
i2c_tmp[4 + i] = (shtpData[i]);
}
WriteNbyte(i2c_tmp, (u8)(dataLength + 4));
return (1);
}bno080.h
#ifndef __BNO08X_H_
#define __BNO08X_H_
#include "main.h"
// The default I2C address for the BNO080 on the SparkX breakout is 0x4B. 0x4A is also possible.
#define BNO080_DEFAULT_ADDRESS 0x4B
#define BNO080_W_ADDRESS ( BNO080_DEFAULT_ADDRESS << 1 | 0x00 )
#define BNO080_R_ADDRESS ( BNO080_DEFAULT_ADDRESS << 1 | 0x01 )
// The catch-all default is 32
#define I2C_BUFFER_LENGTH 32
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// Registers
#define CHANNEL_COMMAND 0
#define CHANNEL_EXECUTABLE 1
#define CHANNEL_CONTROL 2
#define CHANNEL_REPORTS 3
#define CHANNEL_WAKE_REPORTS 4
#define CHANNEL_GYRO 5
// All the ways we can configure or talk to the BNO080, figure 34, page 36 reference manual
// These are used for low level communication with the sensor, on channel 2
#define SHTP_REPORT_COMMAND_RESPONSE 0xF1
#define SHTP_REPORT_COMMAND_REQUEST 0xF2
#define SHTP_REPORT_FRS_READ_RESPONSE 0xF3
#define SHTP_REPORT_FRS_READ_REQUEST 0xF4
#define SHTP_REPORT_PRODUCT_ID_RESPONSE 0xF8
#define SHTP_REPORT_PRODUCT_ID_REQUEST 0xF9
#define SHTP_REPORT_BASE_TIMESTAMP 0xFB
#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD
// All the different sensors and features we can get reports from
// These are used when enabling a given sensor
#define SENSOR_REPORTID_ACCELEROMETER 0x01
#define SENSOR_REPORTID_GYROSCOPE 0x02
#define SENSOR_REPORTID_MAGNETIC_FIELD 0x03
#define SENSOR_REPORTID_LINEAR_ACCELERATION 0x04
#define SENSOR_REPORTID_ROTATION_VECTOR 0x05
#define SENSOR_REPORTID_GRAVITY 0x06
#define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08
#define SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR 0x09
#define SENSOR_REPORTID_TAP_DETECTOR 0x10
#define SENSOR_REPORTID_STEP_COUNTER 0x11
#define SENSOR_REPORTID_STABILITY_CLASSIFIER 0x13
#define SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER 0x1E
// Record IDs from figure 29, page 29 reference manual
// These are used to read the metadata for each sensor type
#define FRS_RECORDID_ACCELEROMETER 0xE302
#define FRS_RECORDID_GYROSCOPE_CALIBRATED 0xE306
#define FRS_RECORDID_MAGNETIC_FIELD_CALIBRATED 0xE309
#define FRS_RECORDID_ROTATION_VECTOR 0xE30B
// Command IDs from section 6.4, page 42
// These are used to calibrate, initialize, set orientation, tare etc the sensor
#define COMMAND_ERRORS 1
#define COMMAND_COUNTER 2
#define COMMAND_TARE 3
#define COMMAND_INITIALIZE 4
#define COMMAND_DCD 6
#define COMMAND_ME_CALIBRATE 7
#define COMMAND_DCD_PERIOD_SAVE 9
#define COMMAND_OSCILLATOR 10
#define COMMAND_CLEAR_DCD 11
#define CALIBRATE_ACCEL 0
#define CALIBRATE_GYRO 1
#define CALIBRATE_MAG 2
#define CALIBRATE_PLANAR_ACCEL 3
#define CALIBRATE_ACCEL_GYRO_MAG 4
#define CALIBRATE_STOP 5
#define MAX_PACKET_SIZE 128 // Packets can be up to 32k but we don't have that much RAM.
#define MAX_METADATA_SIZE 9 // This is in words. There can be many but we mostly only care about the first 9 (Qs, range, etc)
void softReset(); // Try to reset the IMU via software
unsigned char resetReason(); // Query the IMU for the reason it last reset
float qToFloat(int16_t fixedPointValue, unsigned char qPoint); // Given a Q value, converts fixed point floating to regular floating point number
// char waitForI2C(); // Delay based polling for I2C traffic
char receivePacket(void);
char getData(unsigned int charsRemaining); // Given a number of chars, send the requests in I2C_BUFFER_LENGTH chunks
char sendPacket(unsigned char channelNumber, unsigned char dataLength);
// void printPacket(void); // Prints the current shtp header and data packets
void enableRotationVector(unsigned int timeBetweenReports);
void enableGameRotationVector(unsigned int timeBetweenReports);
void enableAccelerometer(unsigned int timeBetweenReports);
void enableLinearAccelerometer(unsigned int timeBetweenReports);
void enableGyro(unsigned int timeBetweenReports);
void enableMagnetometer(unsigned int timeBetweenReports);
void enableStepCounter(unsigned int timeBetweenReports);
void enableStabilityClassifier(unsigned int timeBetweenReports);
char dataAvailable(void);
void parseInputReport(void);
float getQuatI();
float getQuatJ();
float getQuatK();
float getQuatReal();
float getQuatRadianAccuracy();
unsigned char getQuatAccuracy();
float getAccelX();
float getAccelY();
float getAccelZ();
unsigned char getAccelAccuracy();
float getLinAccelX();
float getLinAccelY();
float getLinAccelZ();
unsigned char getLinAccelAccuracy();
float getGyroX();
float getGyroY();
float getGyroZ();
unsigned char getGyroAccuracy();
float getMagX();
float getMagY();
float getMagZ();
unsigned char getMagAccuracy();
void calibrateAccelerometer();
void calibrateGyro();
void calibrateMagnetometer();
void calibratePlanarAccelerometer();
void calibrateAll();
void endCalibration();
void saveCalibration();
unsigned int getStepCount();
unsigned char getStabilityClassifier();
unsigned char getActivityClassifier();
void setFeatureCommand(unsigned char reportID, unsigned int timeBetweenReports, unsigned long specificConfig);
void sendCommand(unsigned char command);
void sendCalibrateCommand(unsigned char thingToCalibrate);
// Metadata functions
int getQ1(unsigned int recordID);
int getQ2(unsigned int recordID);
int getQ3(unsigned int recordID);
float getResolution(unsigned int recordID);
float getRange(unsigned int recordID);
unsigned long readFRSword(unsigned int recordID, unsigned char wordNumber);
void frsReadRequest(unsigned int recordID, unsigned int readOffset, unsigned int blockSize);
char readFRSdata(unsigned int recordID, unsigned char startLocation, unsigned char wordsToRead);
void I2C_Init(void);
#endif作者：lrj depression
