目录
一. 机器人掉电保存参数的原理
由于水下机器人的PID参数众多,算一下三轴角度环、三轴角速度环、深度环,一共有7组PID也就是21个参数,不可能烧一次代码看一下运动效果,然后修改参数再烧代码再看效果,这样效率很低而且完全没有必要。所以必须要实现在线调参与参数掉电保存。那么如何实现呢?只需要在FLASH中开辟一块内存区域,专门存放参数,每次机器人上电运行时首先从这块内存区域中读取参数,然后进入工作状态。在运行过程中,通过串口给板子发送新的参数,控制板将内存中的参数更新,然后重新载入参数即可。这样数据就保存在了内存中,实现了掉电保存。
所以其实关键就是要将参数写入存储器中,可以是内部的FLASH、SDRAM,也可以是外接的FLASH、RAM、内存卡等。 这里我是写入了FLASH,下面看一看如何实现。(参考正点原子的例程)。
二. 实现水下四旋翼PID参数掉电保存
(一)STM32F767 FLASH原理
存储器的组织:
不同型号的STM32芯片其FLASH容量和闪存模块组织是不一样的,这个需要查手册,STM32F767 IGT6 的FLASH容量为1M也就是1024KB,其模块组织如下表所示。
STM32F767IGT6的闪存模块由:主存储器、系统存储器、OPT区域和选项字节等4部分组成
主存储器,该部分用来存放代码和数据常数(如 const类型的数据)。它可以分为1个Bank或者2个Bank,可以通过选项字节的 nBANK位来设置,默认是一个Bank的,表所示即单Bank的闪存组织结构。STM32F767的主存储器被分为8个扇区,前4个扇区为32KB大小,第五个扇区是128KB大小,剩下的3个扇区都是256KB大小,总共1M字节。
因为STM32F7的 FLASH访问路径有两条:AXIM和ITCM,对应不同的地址映射,表中列出了这两条不同访问路径下的扇区地址范围。一般选择AXM接口访问 FLASH,其主存储器的起始地址就是0X08000000
系统存储器,这个主要用来存放STM32F767的 bootloader代码,此代码是出厂的时候就固化在STM32F767里面了,专门来给主存储器下载代码的。
在执行闪存写操作时,任何对闪存的读操作都会锁住总线,在写操作完成后读操作才能正确地进行;既在进行写或擦除操作时,不能进行代码或数据的读取操作。
内存的读取
STM23F7的 FLASH读取是很简单的。例如,我们要从地址adr,读取一个字(一个字为32位),可以通过如下的语句读取
data = (vu32)addr;
将addr强制转换为vu32指针,然后取该指针所指向的地址的值,即得到了addr地址的值。
类似的,将上面的vu32改为vu8,即可读取指定地址的一个字节。
内存的擦除和写入
STM32的flash可以扇区擦除和整片擦除,我们主要用扇区擦除。每次写入数据都必须先擦除数据所在的整片扇区(感觉这个很坑)。
几个主要的函数:
(1)锁定解锁函数:
HAL_Status TypeDef HAL_FLASH_Unlock(void); //解锁函数
HAL_Status TypeDef HAL_FLASH_Lock(void); //锁定函数
每次flash编程开始调用解锁函数,编程完成之后调用锁定函数
(2)写操作函数
HAL_StatusTypeDef HAL_FLASH_Program(uint32_t TypeProgram, uint32_t Address,uint64_t Data); //FLASH 写操作函数
该函数有三个入口参数。 入口参数 TypeProgram 用来区分要写入的数据类型,取值为:FLASH_TYPEPROGRAM_BYTE(字节: 8 位), FLASH_TYPEPROGRAM_HALFWORD(半
字 : 16 位 ) , FLASH_TYPEPROGRAM_WORD ( 字 : 32 位 ) 和FLASH_TYPEPROGRAM_DOUBLEWORD(双字: 64 位),用户根据写入数据类型选择即可。第二个入口参数 Address 用来设置要写入数据的 FLASH 地址。第三个入口参数 Data 顾名思义就是要写入的数据类型,这个参数默认是 64 位的,如果你要写入小于 64 位的数据比如 16 位,程序会进行类型转换。
(3) 擦除函数
HAL 库提供的擦除函数在 stm32f7xx_hal_flash_ex.c 中定义。和编程函数一样, HAL 提供
了一个通用的基于小区擦除的函数 HAL_FLASHEx_Erase,该函数声明如下:
HAL_StatusTypeDef HAL_FLASHEx_Erase(FLASH_EraseInitTypeDef *pEraseInit,uint32_t *SectorError);
该函数有2个入口参数,第一个入口参数pEraseInit 是FLASH_EraseInitTypeDef 结构体指针类型,结构体 FLASH_EraseInitTypeDef 定义如下:
typedef struct
{
uint32_t TypeErase; //擦除类型
#if defined (FLASH_OPTCR_nDBANK)
uint32_t Banks; //擦除的 Bank 编号
#endif
uint32_t Sector; //擦除的 sector 号
uint32_t NbSectors; //擦除的 sector 数量
uint32_t VoltageRange; //电压范围
} FLASH_EraseInitTypeDef;
(4) 等待操作完成函数
在执行闪存写操作时,任何对闪存的读操作都会锁住总线,在写操作完成后读操作才能正
确地进行;既在进行写或擦除操作时,不能进行代码或数据的读取操作。所以在每次操作之前,都要等待上一次操作完成这次操作才能开始。 HAL 库函数为:
HAL_StatusTypeDef FLASH_WaitForLastOperation(uint32_t Timeout);
该函数在 HAL 库中很多地方用到,比如擦除函数 HAL_FLASHEx_Erase 中在对 FLASH 进
行擦除操作后会调用该函数,等待擦除操作完成。
(5) 读数据函数
读取 FLASH 指定地址的数据的函数固件库并没有给出来,可以自己定义一个读取一个字的函数:
u32 STMFLASH_ReadWord(u32 faddr)
{
return *(vu32*)faddr;
}
(二)FLASH读写参数实现
在前面工程的基础上,新建STMFlash.c
和STMFlash.h
文件,STMFlash.h
内容如下:
#ifndef __STMFLASH_H
#define __STMFLASH_H
#include "sys.h"
#include "pid.h"
//FLASH起始地址
#define STM32_FLASH_BASE 0x08000000 //STM32 FLASH的起始地址
#define FLASH_WAITETIME 50000 //FLASH等待超时时间
#define FIRMWARE_SIZE (96*1024) // 程序存储区,80K左右,预留128K
#define FIRMWARE_START_ADDR (FLASH_BASE)
// 数据保存在 ADDR_FLASH_SECTOR_3 程序大小约为74KB
#define CONFIG_PARAM_ADDR (FLASH_BASE + FIRMWARE_SIZE)
//STM32F767 FLASH 扇区的起始地址
#define ADDR_FLASH_SECTOR_0 ((uint32_t)0x08000000) //扇区0起始地址, 32 Kbytes
#define ADDR_FLASH_SECTOR_1 ((uint32_t)0x08008000) //扇区1起始地址, 32 Kbytes
#define ADDR_FLASH_SECTOR_2 ((uint32_t)0x08010000) //扇区2起始地址, 32 Kbytes
#define ADDR_FLASH_SECTOR_3 ((uint32_t)0x08018000) //扇区3起始地址, 32 Kbytes
#define ADDR_FLASH_SECTOR_4 ((uint32_t)0x08020000) //扇区4起始地址, 128 Kbytes
#define ADDR_FLASH_SECTOR_5 ((uint32_t)0x08040000) //扇区5起始地址, 256 Kbytes
#define ADDR_FLASH_SECTOR_6 ((uint32_t)0x08080000) //扇区6起始地址, 256 Kbytes
#define ADDR_FLASH_SECTOR_7 ((uint32_t)0x080C0000) //扇区7起始地址, 256 Kbytes
u32 STMFLASH_ReadWord(u32 faddr); //读出字
void STMFLASH_Write(u32 WriteAddr,u32 *pBuffer,u32 NumToWrite); //从指定地址开始写入指定长度的数据
void STMFLASH_Read(u32 ReadAddr,u32 *pBuffer,u32 NumToRead); //从指定地址开始读出指定长度的数据
u8 configParamCksum(configParam_t* data);
#endif
头文件中定义了各个扇区的起始地址,定义程序代码的起始地址为基地址,由于我的代码约为74KB,所以前三个扇区留给代码区,数据存储放在了扇区3(ADDR_FLASH_SECTOR_3
,地址为0x08018000
)。
STMFlash.c
内容如下:
#include "stmflash.h"
#include "delay.h"
//读取指定地址的字(32位数据)
//faddr:读地址
//返回值:对应数据.
u32 STMFLASH_ReadWord(u32 faddr)
{
return *(__IO uint32_t *)faddr;
}
//获取某个地址所在的flash扇区
//addr:flash地址
//返回值:0~11,即addr所在的扇区
uint16_t STMFLASH_GetFlashSector(u32 addr)
{
if(addr<ADDR_FLASH_SECTOR_1)return FLASH_SECTOR_0;
else if(addr<ADDR_FLASH_SECTOR_2)return FLASH_SECTOR_1;
else if(addr<ADDR_FLASH_SECTOR_3)return FLASH_SECTOR_2;
else if(addr<ADDR_FLASH_SECTOR_4)return FLASH_SECTOR_3;
else if(addr<ADDR_FLASH_SECTOR_5)return FLASH_SECTOR_4;
else if(addr<ADDR_FLASH_SECTOR_6)return FLASH_SECTOR_5;
else if(addr<ADDR_FLASH_SECTOR_7)return FLASH_SECTOR_6;
return FLASH_SECTOR_7;
}
//从指定地址开始写入指定长度的数据
//特别注意:因为STM32F7的扇区实在太大,没办法本地保存扇区数据,所以本函数
// 写地址如果非0XFF,那么会先擦除整个扇区且不保存扇区数据.所以
// 写非0XFF的地址,将导致整个扇区数据丢失.建议写之前确保扇区里
// 没有重要数据,最好是整个扇区先擦除了,然后慢慢往后写.
//该函数对OTP区域也有效!可以用来写OTP区!
//OTP区域地址范围:0X1FF0F000~0X1FF0F41F
//WriteAddr:起始地址(此地址必须为4的倍数!!)
//pBuffer:数据指针
//NumToWrite:字(32位)数(就是要写入的32位数据的个数.)
void STMFLASH_Write(u32 WriteAddr,u32 *pBuffer,u32 NumToWrite)
{
FLASH_EraseInitTypeDef FlashEraseInit;
HAL_StatusTypeDef FlashStatus=HAL_OK;
u32 SectorError=0;
u32 addrx=0;
u32 endaddr=0;
if(WriteAddr<STM32_FLASH_BASE||WriteAddr%4)return; //非法地址
HAL_FLASH_Unlock(); //解锁
addrx=WriteAddr; //写入的起始地址
endaddr=WriteAddr+NumToWrite*4; //写入的结束地址
if(addrx<0X1FF00000)
{
while(addrx<endaddr) //扫清一切障碍.(对非FFFFFFFF的地方,先擦除)
{
if(STMFLASH_ReadWord(addrx)!=0XFFFFFFFF)//有非0XFFFFFFFF的地方,要擦除这个扇区
{
FlashEraseInit.TypeErase=FLASH_TYPEERASE_SECTORS; //擦除类型,扇区擦除
FlashEraseInit.Sector=STMFLASH_GetFlashSector(addrx); //要擦除的扇区
FlashEraseInit.NbSectors=1; //一次只擦除一个扇区
FlashEraseInit.VoltageRange=FLASH_VOLTAGE_RANGE_3; //电压范围,VCC=2.7~3.6V之间!!
if(HAL_FLASHEx_Erase(&FlashEraseInit,&SectorError)!=HAL_OK)
{
break;//发生错误了
}
SCB_CleanInvalidateDCache(); //清除无效的D-Cache
}else addrx+=4;
FLASH_WaitForLastOperation(FLASH_WAITETIME); //等待上次操作完成
}
}
FlashStatus=FLASH_WaitForLastOperation(FLASH_WAITETIME); //等待上次操作完成
if(FlashStatus==HAL_OK)
{
while(WriteAddr<endaddr)//写数据
{
if(HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,WriteAddr,*pBuffer)!=HAL_OK)//写入数据
{
break; //写入异常
}
WriteAddr+=4;
pBuffer++;
}
}
HAL_FLASH_Lock(); //上锁
}
//从指定地址开始读出指定长度的数据
//ReadAddr:起始地址
//pBuffer:数据指针
//NumToRead:字(32位)数
void STMFLASH_Read(u32 ReadAddr,u32 *pBuffer,u32 NumToRead)
{
u32 i;
for(i=0;i<NumToRead;i++)
{
pBuffer[i]=STMFLASH_ReadWord(ReadAddr);//读取4个字节.
ReadAddr+=4;//偏移4个字节.
}
}
主要看擦除和写入函数void STMFLASH_Write(u32 WriteAddr,u32 *pBuffer,u32 NumToWrite)
,首先判断地址是否是4的倍数,地址合法之后,解锁flash,然后获取写入的起始地址和结束地址,执行写入操作。写入过程首先要将非0xFFFFFFFF
地方擦除,然后调用HAL_FLASH_Program函数
写入数据。
(三)自定义固件参数结构体
现在我们需要搭坐时光机,回到之前的一篇文章,请点进我的主页打开STM32实现水下四旋翼(五)自定义航行数据,我们当时在pid.c
和pid.h
中定义了一个固件参数的结构体,一起再来看一下:
pid.h
中定义了一个参数结构体configParam_t
:
typedef struct
{
float kp;
float ki;
float kd;
} pidInit_t;
typedef struct
{
pidInit_t roll;
pidInit_t pitch;
pidInit_t yaw;
} pidParam_t;
typedef struct
{
pidInit_t vx;
pidInit_t vy;
pidInit_t vz;
} pidParamPos_t;
typedef struct
{
pidParam_t pidAngle; /*角度PID*/
pidParam_t pidRate; /*角速度PID*/
pidParamPos_t pidPos; /*位置PID*/
float thrustBase; /*油门基础值*/
u8 cksum;
} configParam_t;
extern configParam_t configParam;
pid.c
中内容如下,定义了参数结构体实例configParam
。后面每次上电就将flash中的数据区的数据读取到configParam
中保存,串口接收到修改数据的命令后也是将新数据更新到configParam
中,然后在任务中循环判断configParam
的参数是否发生了改变,如果发生了改变就将数据写入到FLASH中的数据区。
怎么判断呢?configParam
中有一个成员变量是校验位u8 cksum
,定义一个计算校验和的函数u8 configParamCksum(configParam_t* data)
,因为每次接收到新数据只会更新数据,而不会更新校验位,前后两次读数据发现校验位不一样则说明数据发生了变化。
configParam_t configParam=
{
.pidAngle= /*角度PID*/
{
.roll=
{
.kp=3.0,
.ki=0.0,
.kd=0.0,
},
.pitch=
{
.kp=3.0,
.ki=0.0,
.kd=0.0,
},
.yaw=
{
.kp=3.0,
.ki=0.0,
.kd=0.0,
},
},
.pidRate= /*角速度PID*/
{
.roll=
{
.kp=30.0,
.ki=0.2,
.kd=0.0,
},
.pitch=
{
.kp=30.0,
.ki=0.2,
.kd=0.0,
},
.yaw=
{
.kp=30.0,
.ki=0.2,
.kd=0.0,
},
},
.pidPos= /*位置PID*/
{
.vx=
{
.kp=0.0,
.ki=0.0,
.kd=0.0,
},
.vy=
{
.kp=0.0,
.ki=0.0,
.kd=0.0,
},
.vz=
{
.kp=35.0,
.ki=0.1,
.kd=0.0,
},
},
.thrustBase = -2000.0,
};
// 获取校验值
u8 configParamCksum(configParam_t* data)
{
int i;
u8 cksum=0;
u8* c = (u8*)data;
size_t len=sizeof(configParam_t);
for (i=0; i<len; i++)
cksum += *(c++);
cksum-=data->cksum;
return cksum;
}
(四)创建数传任务Transmission_task
现在我们需要创建一个新的任务了,这个任务专门用来传输数据,包括上传和下传,上传的是机器人的姿态、电压、速度等数据,下传的是调参等命令。打开main 函数,按照添加任务的流程,首先添加任务堆栈、任务函数的等定义:
//config 参数配置任务 在线调试参数并写入flash
//设置任务优先级
#define TRANSMISSION_TASK_PRIO 6 // 这个任务的优先级对串口收发的速度影响很大,不可太低,更改之后要重新测试串口3通信
//任务堆栈大小
#define TRANSMISSION_STK_SIZE 1024
//任务控制块
OS_TCB TransmissionTaskTCB;
//任务堆栈
CPU_STK TRANSMISSION_TASK_STK[TRANSMISSION_STK_SIZE];
//motor任务
u8 transmission_task(void *p_arg);
在开始任务函数void start_task(void *p_arg)
中添加创建任务:
OSTaskCreate((OS_TCB *)&TransmissionTaskTCB,
(CPU_CHAR *)"Transmission task",
(OS_TASK_PTR)transmission_task,
(void *)0,
(OS_PRIO)TRANSMISSION_TASK_PRIO,
(CPU_STK *)&TRANSMISSION_TASK_STK[0],
(CPU_STK_SIZE)TRANSMISSION_STK_SIZE / 10,
(CPU_STK_SIZE)TRANSMISSION_STK_SIZE,
(OS_MSG_QTY)0,
(OS_TICK)10,
(void *)0,
(OS_OPT)OS_OPT_TASK_STK_CHK | OS_OPT_TASK_STK_CLR | OS_OPT_TASK_SAVE_FP,
(OS_ERR *)&err);
然后添加任务函数
// 在线调试参数用
// 上传参数到地面站
u8 transmission_task(void *p_arg)
{
OS_ERR err;
CPU_SR_ALLOC();
u8 lenth;
lenth = sizeof(configParam);
lenth = lenth / 4 + (lenth % 4 ? 1 : 0);
STMFLASH_Read(CONFIG_PARAM_ADDR, (u32 *)&configParam, lenth);
while (1)
{
uploadPID();
uploadStateInfo();
uploadMotoInfo();
uploadPowerAndWaterInfo();
uploadIMUInfo();
uploadGPSInfo();
// // 校验值不一致,说明参数发生了改变,更新到flash中
if (configParamCksum(&configParam) != configParam.cksum) // 参数的校验值
{
printf("enter this progress\r\n");
configParam.cksum = configParamCksum(&configParam); // 更新校验值
STMFLASH_Write(CONFIG_PARAM_ADDR, (u32 *)&configParam, lenth); /*写入stm32 flash*/
attitudeResetAllPID(); //PID复位
positionResetAllPID();
attitudeControlInit(); // 用新的参数重新初始化pid
positionControlInit();
}
delay_ms(20);
}
}
数传任务的内容很好理解了,首先读一次数据,从参数区读取参数,系统开始工作,然后在while循环中定期上传数据,同时每次都判断数据是否发生了改变,如果发生了改变则更新数据到FLASH中。更新数据是在哪里呢,这个是在串口的中断函数里面实现的,后面我会继续讲。至于while循环中的upload开头的函数都是上传数据,从名字就可以看出是上传不同的数据帧,后面会专门讲解。
好的,调参的代码已经实现了,下一讲一起写一个上位机地面站,用来实现数据可视化。