STM32实现水下四旋翼（十二）数传任务1——PID参数掉电保存

一. 机器人掉电保存参数的原理

由于水下机器人的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开头的函数都是上传数据，从名字就可以看出是上传不同的数据帧，后面会专门讲解。

好的，调参的代码已经实现了，下一讲一起写一个上位机地面站，用来实现数据可视化。