有几年没做STM32了，这几天闲来没事调个驱动
1.CUBEMX配置
配置时钟
配置SWD接口
配置IIC
串口不配了，懒得折腾，直接DEBUG看变量就行
2.驱动程序
吐槽一下，STM32 IDE好像有点严格，我买的盗版ST LINK，竟然死活用不了，没法子，只能搞个MDK，好在是自己私人用，公司用的话估计又要收律师函了。。。。
hmc5883.c
/* USER CODE BEGIN Header */
/**
******************************************************************************
* File Name : hmc5883l.c
* Description : I2C drive based on STM32F4
* STM32 HAL library ver: STM32Cube_FW_F4_V1.27.1
*
******************************************************************************
* @attention
*
* Copyright (c) 2024~2029 mingfei.tang
* All rights reserved.
*
*************************************************************************
*/
/* USER CODE END Header */
#include "hmc5883l.h"
#include <math.h>
#include <string.h>
HMC5883L_T g_tMag;
Compass_T g_tCompass;
CompassDebug_T g_tCompassDebug;
/* 调试信息初始化 */
void Compass_InitDebugInfo(void)
{
g_tCompassDebug.heading = 0.0f;
g_tCompassDebug.direction = DIR_NORTH;
g_tCompassDebug.calProgress = 0.0f;
g_tCompassDebug.calState = CAL_IDLE;
g_tCompassDebug.rawX = 0;
g_tCompassDebug.rawY = 0;
g_tCompassDebug.rawZ = 0;
g_tCompassDebug.calX = 0.0f;
g_tCompassDebug.calY = 0.0f;
g_tCompassDebug.calZ = 0.0f;
g_tCompassDebug.isNorthFound = 0;
g_tCompassDebug.isDataReady = 0;
strcpy(g_tCompassDebug.directionStr, "CAL");
}
static uint8_t hmc5883L_WeReg( uint16_t regAdd, uint8_t *pData, uint16_t Size )
{
HAL_StatusTypeDef status;
status = HAL_I2C_Mem_Write( &hi2c1, HMC5883L_SLAVE_ADDRESS, regAdd,
I2C_MEMADD_SIZE_8BIT, pData, Size, 1000);
if( status == HAL_OK)
return HMC5883L_OK;
else
return HMC5883L_ERROR;
}
static uint8_t hmc5883L_RdReg( uint16_t regAdd, uint8_t *pData, uint16_t Size )
{
HAL_StatusTypeDef status;
status = HAL_I2C_Mem_Read( &hi2c1, HMC5883L_SLAVE_ADDRESS, regAdd,
I2C_MEMADD_SIZE_8BIT, pData, Size, 1000);
if( status == HAL_OK)
return HMC5883L_OK;
else
return HMC5883L_ERROR;
}
void hmc5883L_WriteByte(uint8_t _ucRegAddr, uint8_t _ucRegData)
{
hmc5883L_WeReg( _ucRegAddr, &_ucRegData, 1);
}
uint8_t hmc5883L_ReadByte(uint8_t _ucRegAddr)
{
uint8_t _ucRegData;
hmc5883L_RdReg( _ucRegAddr, &_ucRegData, 1);
return _ucRegData;
}
/* 初始化校准数据结构 */
void Compass_InitCalibration(void)
{
g_tCompass.calState = CAL_IDLE;
g_tCompass.sampleCount = 0;
g_tCompass.isCalibrated = 0;
g_tCompass.declinationAngle = -6.5f; // 北京磁偏角，根据地理位置调整
g_tCompass.offsetX = 0.0f;
g_tCompass.offsetY = 0.0f;
g_tCompass.offsetZ = 0.0f;
g_tCompass.scaleX = 1.0f;
g_tCompass.scaleY = 1.0f;
g_tCompass.scaleZ = 1.0f;
g_tCompass.maxX = -32768;
g_tCompass.minX = 32767;
g_tCompass.maxY = -32768;
g_tCompass.minY = 32767;
g_tCompass.maxZ = -32768;
g_tCompass.minZ = 32767;
/* 自动寻找磁北的变量 */
g_tCompass.northX = 0.0f;
g_tCompass.northY = 0.0f;
g_tCompass.northFound = 0;
g_tCompass.autoCalStartTime = 0;
g_tCompass.autoCalSamples = 0;
}
/* 开始自动校准（寻找磁北） */
void Compass_StartAutoCalibration(void)
{
g_tCompass.calState = CAL_START;
g_tCompass.isCalibrated = 0;
g_tCompass.northFound = 0;
g_tCompass.autoCalSamples = 0;
g_tCompass.maxX = -32768;
g_tCompass.minX = 32767;
g_tCompass.maxY = -32768;
g_tCompass.minY = 32767;
g_tCompass.maxZ = -32768;
g_tCompass.minZ = 32767;
/* 用于寻找磁北的变量 */
g_tCompass.autoCalStartTime = HAL_GetTick();
/* 更新调试信息 */
g_tCompassDebug.calState = CAL_START;
g_tCompassDebug.calProgress = 0.0f;
g_tCompassDebug.isNorthFound = 0;
}
/* 自动校准算法 - 通过收集旋转数据寻找磁北 */
void Compass_AutoCalibrateAlgorithm(int16_t x, int16_t y, int16_t z)
{
static float sampleX[100]; // 存储样本用于分析
static float sampleY[100];
static uint8_t sampleIdx = 0;
if (g_tCompass.calState != CAL_START || g_tCompass.northFound) {
return;
}
/* 保存当前样本 */
sampleX[sampleIdx] = (float)x;
sampleY[sampleIdx] = (float)y;
sampleIdx++;
if (sampleIdx >= 100) {
sampleIdx = 0;
}
g_tCompass.autoCalSamples++;
/* 计算校准进度 */
float progress = (float)g_tCompass.autoCalSamples / AUTO_CAL_SAMPLES * 100.0f;
if (progress > 100.0f) progress = 100.0f;
g_tCompassDebug.calProgress = progress;
/* 收集足够样本后，开始分析数据寻找磁北 */
if (g_tCompass.autoCalSamples >= AUTO_CAL_SAMPLES) {
/* 方法1：寻找最大X值（假设X轴指向磁北时读数最大） */
float maxX = -999999.0f;
float maxY = 0.0f;
for (int i = 0; i < 100; i++) {
if (sampleX[i] > maxX) {
maxX = sampleX[i];
maxY = sampleY[i];
}
}
/* 设置磁北方向 */
g_tCompass.northX = maxX;
g_tCompass.northY = maxY;
g_tCompass.northFound = 1;
g_tCompass.isCalibrated = 1;
g_tCompass.calState = CAL_COMPLETED;
/* 更新调试信息 */
g_tCompassDebug.calState = CAL_COMPLETED;
g_tCompassDebug.isNorthFound = 1;
g_tCompassDebug.calProgress = 100.0f;
/* 计算硬铁偏移（椭圆拟合法简化版） */
float avgX = 0.0f, avgY = 0.0f;
for (int i = 0; i < 100; i++) {
avgX += sampleX[i];
avgY += sampleY[i];
}
avgX /= 100.0f;
avgY /= 100.0f;
g_tCompass.offsetX = avgX;
g_tCompass.offsetY = avgY;
/* 计算软铁比例 */
float maxDist = 0.0f;
for (int i = 0; i < 100; i++) {
float dx = sampleX[i] - avgX;
float dy = sampleY[i] - avgY;
float dist = sqrtf(dx*dx + dy*dy);
if (dist > maxDist) maxDist = dist;
}
/* 归一化比例因子 */
if (maxDist > 0) {
g_tCompass.scaleX = 1000.0f / maxDist; // 归一化到固定范围
g_tCompass.scaleY = 1000.0f / maxDist;
}
}
}
/* 计算相对于磁北的航向 */
float Compass_CalculateHeadingRelativeToNorth(float x, float y, float z)
{
if (!g_tCompass.isCalibrated || !g_tCompass.northFound) {
return 0.0f; // 未校准，返回0
}
/* 应用校准 */
float calX = (x - g_tCompass.offsetX) * g_tCompass.scaleX;
float calY = (y - g_tCompass.offsetY) * g_tCompass.scaleY;
/* 计算相对于磁北的方向 */
float dx = calX - g_tCompass.northX;
float dy = calY - g_tCompass.northY;
float heading = atan2f(dy, dx) * 180.0f / M_PI;
/* 转换为0-360度 */
if (heading < 0) heading += 360.0f;
/* 应用磁偏角校正（转换为地理北） */
heading += g_tCompass.declinationAngle;
/* 保持在0-360度范围内 */
if (heading >= 360.0f) heading -= 360.0f;
if (heading < 0.0f) heading += 360.0f;
return heading;
}
/* 获取指南针方向（东南西北） */
CompassDirection_T Compass_GetDirection(float heading)
{
/* 将0-360度转换为方向 */
if ((heading >= 337.5) || (heading < 22.5))
return DIR_NORTH;
else if (heading < 67.5)
return DIR_NORTHEAST;
else if (heading < 112.5)
return DIR_EAST;
else if (heading < 157.5)
return DIR_SOUTHEAST;
else if (heading < 202.5)
return DIR_SOUTH;
else if (heading < 247.5)
return DIR_SOUTHWEST;
else if (heading < 292.5)
return DIR_WEST;
else
return DIR_NORTHWEST;
}
/* 获取方向字符串 */
const char* Compass_GetDirectionString(CompassDirection_T dir)
{
static const char* dirStrings[] = {
"N", "NE", "E", "SE", "S", "SW", "W", "NW"
};
if (dir < 8) {
return dirStrings[dir];
}
return "CAL";
}
/* 更新调试信息中的方向字符串 */
void Compass_UpdateDirectionString(CompassDirection_T dir)
{
const char* dirStr = Compass_GetDirectionString(dir);
strncpy(g_tCompassDebug.directionStr, dirStr, sizeof(g_tCompassDebug.directionStr)-1);
g_tCompassDebug.directionStr[sizeof(g_tCompassDebug.directionStr)-1] = '\0';
}
void hmc5883l_Init(void)
{
/* 设置Mode寄存器 */
#if 1
hmc5883L_WriteByte(0x00, 0x70);
hmc5883L_WriteByte(0x01, 0x20);
hmc5883L_WriteByte(0x02, 0x00);
#else /* 自校准模式 */
hmc5883L_WriteByte(0x00, 0x70 + 2);
hmc5883L_WriteByte(0x01, 0x20);
hmc5883L_WriteByte(0x02, 0x00);
#endif
g_tMag.CfgRegA = hmc5883L_ReadByte(0);
g_tMag.CfgRegB = hmc5883L_ReadByte(1);
g_tMag.ModeReg = hmc5883L_ReadByte(2);
g_tMag.IDReg[0] = hmc5883L_ReadByte(10);
g_tMag.IDReg[1] = hmc5883L_ReadByte(11);
g_tMag.IDReg[2] = hmc5883L_ReadByte(12);
g_tMag.IDReg[3] = 0;
/* 设置最小最大值初值 */
g_tMag.X_Min = 32767;
g_tMag.X_Max = -32768;
g_tMag.Y_Min = 32767;
g_tMag.Y_Max = -32768;
g_tMag.Z_Min = 32767;
g_tMag.Z_Max = -32768;
/* 初始化指南针校准和调试信息 */
Compass_InitCalibration();
Compass_InitDebugInfo();
}
void hmc5883l_ReadData(void)
{
uint8_t ucReadBuf[7];
if (hmc5883L_RdReg(DATA_OUT_X, ucReadBuf, 7) != HMC5883L_OK) {
return; /* 读取失败 */
}
/* 将读出的数据保存到全局结构体变量 */
g_tMag.X = (int16_t)((ucReadBuf[0] << 8) + ucReadBuf[1]);
g_tMag.Z = (int16_t)((ucReadBuf[2] << 8) + ucReadBuf[3]);
g_tMag.Y = (int16_t)((ucReadBuf[4] << 8) + ucReadBuf[5]);
g_tMag.Status = ucReadBuf[6];
/* 保存原始数据到调试变量 */
g_tCompassDebug.rawX = g_tMag.X;
g_tCompassDebug.rawY = g_tMag.Y;
g_tCompassDebug.rawZ = g_tMag.Z;
/* 统计最大值和最小值（用于自动校准） */
if (g_tMag.X > g_tMag.X_Max) {
g_tMag.X_Max = g_tMag.X;
}
if (g_tMag.X < g_tMag.X_Min) {
g_tMag.X_Min = g_tMag.X;
}
if (g_tMag.Y > g_tMag.Y_Max) {
g_tMag.Y_Max = g_tMag.Y;
}
if (g_tMag.Y < g_tMag.Y_Min) {
g_tMag.Y_Min = g_tMag.Y;
}
if (g_tMag.Z > g_tMag.Z_Max) {
g_tMag.Z_Max = g_tMag.Z;
}
if (g_tMag.Z < g_tMag.Z_Min) {
g_tMag.Z_Min = g_tMag.Z;
}
}
/* 获取校准后的磁力计数据 */
void hmc5883l_GetCalibratedData(float *x, float *y, float *z)
{
/* 转换为浮点数 */
*x = (float)g_tMag.X;
*y = (float)g_tMag.Y;
*z = (float)g_tMag.Z;
/* 应用校准 */
if (g_tCompass.isCalibrated) {
*x = (*x - g_tCompass.offsetX) * g_tCompass.scaleX;
*y = (*y - g_tCompass.offsetY) * g_tCompass.scaleY;
*z = (*z - g_tCompass.offsetZ) * g_tCompass.scaleZ;
/* 保存到调试变量 */
g_tCompassDebug.calX = *x;
g_tCompassDebug.calY = *y;
g_tCompassDebug.calZ = *z;
} else {
/* 未校准时，直接使用原始数据 */
g_tCompassDebug.calX = *x;
g_tCompassDebug.calY = *y;
g_tCompassDebug.calZ = *z;
}
}
/* 获取航向角 */
float hmc5883l_GetHeading(void)
{
float x, y, z;
/* 获取校准后的数据 */
hmc5883l_GetCalibratedData(&x, &y, &z);
/* 计算航向（相对于磁北） */
float heading = Compass_CalculateHeadingRelativeToNorth(x, y, z);
g_tCompassDebug.heading = heading;
return heading;
}
/* 获取指南针方向 */
CompassDirection_T hmc5883l_GetDirection(void)
{
float heading = hmc5883l_GetHeading();
CompassDirection_T dir = Compass_GetDirection(heading);
g_tCompassDebug.direction = dir;
Compass_UpdateDirectionString(dir);
g_tCompassDebug.isDataReady = 1; /* 标记数据就绪 */
return dir;
}
/* 自动寻找磁北的指南针测试函数 */
void hmc5883l_CompassTest(void)
{
hmc5883l_Init();
/* 阶段1：自动启动寻找磁北 */
Compass_StartAutoCalibration();
uint32_t startTime = HAL_GetTick();
uint8_t calibrationPhase = 1; // 1=寻找磁北, 2=正常使用
while(1)
{
hmc5883l_ReadData();
uint32_t currentTime = HAL_GetTick();
if (calibrationPhase == 1) {
/* 阶段1：自动寻找磁北 */
/* 运行自动校准算法 */
Compass_AutoCalibrateAlgorithm(g_tMag.X, g_tMag.Y, g_tMag.Z);
/* 更新调试信息 */
g_tCompassDebug.calState = g_tCompass.calState;
g_tCompassDebug.isNorthFound = g_tCompass.northFound;
/* 检查校准是否完成 */
if (g_tCompass.northFound) {
calibrationPhase = 2; // 进入正常使用阶段
startTime = currentTime; // 重置计时器
}
/* 超时保护（10秒内未找到磁北，强制进入下一阶段） */
if ((currentTime - startTime) > 10000) {
calibrationPhase = 2;
g_tCompass.calState = CAL_COMPLETED;
g_tCompass.isCalibrated = 1; // 强制标记为已校准
}
}
else {
/* 阶段2：正常使用（显示航向） */
/* 每200ms更新一次航向 */
if ((currentTime - startTime) > 200) {
startTime = currentTime;
/* 获取航向和方向 */
float heading = hmc5883l_GetHeading();
CompassDirection_T dir = hmc5883l_GetDirection();
/* 更新调试信息 */
g_tCompassDebug.heading = heading;
g_tCompassDebug.direction = dir;
Compass_UpdateDirectionString(dir);
}
}
HAL_Delay(10);
}
}
/* 原始测试函数 */
void hmc5883l_test(void)
{
hmc5883l_Init();
/* 自动启动寻找磁北 */
Compass_StartAutoCalibration();
uint32_t lastUpdateTime = HAL_GetTick();
uint8_t isCalibrated = 0;
while(1)
{
hmc5883l_ReadData();
/* 运行自动校准算法 */
Compass_AutoCalibrateAlgorithm(g_tMag.X, g_tMag.Y, g_tMag.Z);
/* 更新校准状态 */
g_tCompassDebug.calState = g_tCompass.calState;
g_tCompassDebug.isNorthFound = g_tCompass.northFound;
/* 每200ms更新一次调试信息 */
if ((HAL_GetTick() - lastUpdateTime) > 200) {
lastUpdateTime = HAL_GetTick();
if (g_tCompass.isCalibrated && g_tCompass.northFound) {
/* 校准完成，计算航向 */
float heading = hmc5883l_GetHeading();
CompassDirection_T dir = hmc5883l_GetDirection();
/* 更新调试信息 */
g_tCompassDebug.heading = heading;
g_tCompassDebug.direction = dir;
Compass_UpdateDirectionString(dir);
}
}
HAL_Delay(10);
}
}
/* End of this file */hmc5883.h
#ifndef __HMC5883L_H
#define __HMC5883L_H
#include "main.h"
#include "i2c.h"
#include <stdint.h>
#include <string.h>
/* 定义常量 */
#define HMC5883L_SLAVE_ADDRESS 0x3C
#define HMC5883L_OK 0
#define HMC5883L_ERROR 1
/* 数学常量定义 */
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
/* 校准相关定义 */
#define AUTO_CAL_SAMPLES 200 /* 自动校准样本数 */
#define CALIBRATION_TIMEOUT 10000 /* 校准超时时间(毫秒) */
/* 寄存器地址 */
#define DATA_OUT_X 0x03
#define DATA_OUT_Z 0x05
#define DATA_OUT_Y 0x07
/* 校准状态 */
typedef enum {
CAL_IDLE = 0,
CAL_START,
CAL_COMPLETED
} CalibrationState_T;
/* 方向枚举 */
typedef enum {
DIR_NORTH = 0,
DIR_NORTHEAST,
DIR_EAST,
DIR_SOUTHEAST,
DIR_SOUTH,
DIR_SOUTHWEST,
DIR_WEST,
DIR_NORTHWEST
} CompassDirection_T;
/* 磁力计数据结构 */
typedef struct {
int16_t X;
int16_t Y;
int16_t Z;
int16_t X_Min;
int16_t X_Max;
int16_t Y_Min;
int16_t Y_Max;
int16_t Z_Min;
int16_t Z_Max;
uint8_t CfgRegA;
uint8_t CfgRegB;
uint8_t ModeReg;
uint8_t Status;
char IDReg[4];
} HMC5883L_T;
/* 指南针校准数据结构 */
typedef struct {
CalibrationState_T calState;
uint32_t autoCalStartTime;
uint32_t autoCalSamples;
uint32_t sampleCount;
uint8_t isCalibrated;
uint8_t northFound;
/* 硬铁偏移校准参数 */
float offsetX;
float offsetY;
float offsetZ;
/* 软铁比例校准参数 */
float scaleX;
float scaleY;
float scaleZ;
/* 磁北方向数据 */
float northX;
float northY;
/* 磁偏角 */
float declinationAngle;
/* 校准数据 */
int16_t maxX, minX;
int16_t maxY, minY;
int16_t maxZ, minZ;
} Compass_T;
/* 指南针调试数据结构 */
typedef struct {
float heading; /* 航向角（0-360度） */
CompassDirection_T direction; /* 方向枚举 */
float calProgress; /* 校准进度（0-100%） */
CalibrationState_T calState; /* 校准状态 */
int16_t rawX, rawY, rawZ; /* 原始数据 */
float calX, calY, calZ; /* 校准后数据 */
uint8_t isNorthFound; /* 是否找到磁北 */
uint8_t isDataReady; /* 数据就绪标志 */
char directionStr[3]; /* 方向字符串 */
} CompassDebug_T;
/* 函数声明 */
void hmc5883l_Init(void);
void hmc5883l_ReadData(void);
void hmc5883l_GetCalibratedData(float *x, float *y, float *z);
float hmc5883l_GetHeading(void);
CompassDirection_T hmc5883l_GetDirection(void);
void hmc5883l_CompassTest(void);
void hmc5883l_test(void);
/* 校准相关函数 */
void Compass_InitCalibration(void);
void Compass_InitDebugInfo(void);
void Compass_StartAutoCalibration(void);
void Compass_AutoCalibrateAlgorithm(int16_t x, int16_t y, int16_t z);
float Compass_CalculateHeadingRelativeToNorth(float x, float y, float z);
CompassDirection_T Compass_GetDirection(float heading);
const char* Compass_GetDirectionString(CompassDirection_T dir);
void Compass_UpdateDirectionString(CompassDirection_T dir);
/* 全局变量声明 */
extern HMC5883L_T g_tMag;
extern Compass_T g_tCompass;
extern CompassDebug_T g_tCompassDebug;
#endif /* __HMC5883L_H */main.c
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_I2C1_Init();
/* USER CODE BEGIN 2 */
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
hmc5883l_CompassTest();
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}非常简单，没几行代码
3测试效果
还算可靠，基本上测得是稳定的