170 lines
3.1 KiB
C
170 lines
3.1 KiB
C
#include "config.h"
|
|
#include "flash.h"
|
|
#include "string.h"
|
|
#define DEBUG
|
|
|
|
#define CONFIG_BASE_ADDRESS (ADDR_FLASH_SECTOR_11)
|
|
|
|
#define DEG_TO_RAD(x) ((x) * (3.141592653 / 180.0)) //角度转弧度
|
|
|
|
|
|
|
|
|
|
#ifdef DEBUG
|
|
|
|
ConfigParam_t param_chassis ={
|
|
|
|
#else
|
|
|
|
static const ConfigParam_t param_chassis ={
|
|
|
|
#endif
|
|
|
|
.chassis = {/**/
|
|
|
|
|
|
.M3508_param = {
|
|
.p = 15.1f,
|
|
.i = 0.02f,
|
|
.d = 3.2f,
|
|
.i_limit = 200.0f,
|
|
.out_limit =6000.0f,
|
|
},
|
|
.chassis_PICKWzPIDIN_param ={
|
|
.p = 5.0f,
|
|
.i = 0.0f,
|
|
.d = 0.15f,
|
|
.i_limit = 0.0f,
|
|
.out_limit =5000.0f,
|
|
},
|
|
.chassis_PICKWzPIDOUT_param ={
|
|
.p = 10.0f,
|
|
.i = 0.02f,
|
|
.d = 0.00f,
|
|
.i_limit = 0.0f,
|
|
.out_limit =5000.0f,
|
|
},
|
|
// .M3508_param = {
|
|
// .p = 10.0f,
|
|
// .i = 0.0f,
|
|
// .d = 0.0f,
|
|
// .i_limit = 0.0f,
|
|
// .out_limit =10000.0f,
|
|
// },
|
|
|
|
////高速那一套
|
|
// .NaviVx_param ={
|
|
// .p = 1.15f,
|
|
// .i = 0.0f,
|
|
// .d = 0.15f,
|
|
// .i_limit = 0.0f,
|
|
// .out_limit =5000.0f,
|
|
// },
|
|
// .NaviVy_param ={
|
|
// .p = 1.15f,
|
|
// .i = 0.0f,
|
|
// .d = 0.15f,
|
|
// .i_limit = 0.0f,
|
|
// .out_limit =5000.0f,
|
|
// },
|
|
// .NaviVw_param ={
|
|
// .p = 1.15f,
|
|
// .i = 0.0f,
|
|
// .d = 0.15f,
|
|
// .i_limit = 0.0f,
|
|
// .out_limit =5000.0f,
|
|
// },
|
|
//
|
|
|
|
//低速那一套
|
|
.NaviVx_param ={
|
|
.p = 1.04f,
|
|
.i = 0.0f,
|
|
.d = 0.15f,
|
|
.i_limit = 0.0f,
|
|
.out_limit =5000.0f,
|
|
},
|
|
.NaviVy_param ={
|
|
.p = 1.1f,
|
|
.i = 0.0f,
|
|
.d = 0.15f,
|
|
.i_limit = 0.0f,
|
|
.out_limit =5000.0f,
|
|
},
|
|
.NaviVw_param ={
|
|
.p = 1.5f,
|
|
.i = 0.0f,
|
|
.d = 0.15f,
|
|
.i_limit = 0.0f,
|
|
.out_limit =5000.0f,
|
|
},
|
|
.Sick_CaliYparam ={
|
|
.p =2.5f,
|
|
.i =0.001f,
|
|
.d =0.1f,
|
|
.i_limit =15.0f,
|
|
.out_limit =500.0f,
|
|
},
|
|
.Sick_CaliXparam ={
|
|
.p =2.5f,
|
|
.i =0.001f,
|
|
.d =0.1f,
|
|
.i_limit =15.0f,
|
|
.out_limit =500.0f,
|
|
|
|
}
|
|
|
|
},
|
|
|
|
|
|
.can = {
|
|
.pitch6020 = BSP_CAN_1,
|
|
.motor3508 = BSP_CAN_1,
|
|
.chassis6020 = BSP_CAN_2,
|
|
.sick = BSP_CAN_2,
|
|
},
|
|
|
|
};
|
|
const ConfigParam_t *Config_ChassisGet(void)
|
|
{
|
|
return ¶m_chassis;
|
|
}
|
|
|
|
|
|
/**
|
|
* \brief 从Flash读取配置信息
|
|
*
|
|
* \param cfg 配置信息
|
|
*/
|
|
void Config_Get(Config_t *cfg) {
|
|
BSP_Flash_ReadBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
|
|
// /* 防止第一次烧写后访问NULL指针 */
|
|
cfg->chassis_config = ¶m_chassis;
|
|
if (cfg->chassis_config == NULL) cfg->chassis_config = ¶m_chassis;
|
|
/* 防止擦除后全为1 */
|
|
if ((uint32_t)(cfg->chassis_config) == UINT32_MAX)
|
|
cfg->chassis_config = ¶m_chassis;
|
|
|
|
if (memcmp(&cfg->cali_088.gyro_offset.x, "\xFF\xFF\xFF\xFF", 4) == 0) {
|
|
cfg->cali_088.gyro_offset.x = 0.0f;
|
|
}
|
|
if (memcmp(&cfg->cali_088.gyro_offset.y, "\xFF\xFF\xFF\xFF", 4) == 0) {
|
|
cfg->cali_088.gyro_offset.y = 0.0f;
|
|
}
|
|
if (memcmp(&cfg->cali_088.gyro_offset.z, "\xFF\xFF\xFF\xFF", 4) == 0) {
|
|
cfg->cali_088.gyro_offset.z = 0.0f;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* \brief 将配置信息写入Flash
|
|
*
|
|
* \param cfg 配置信息
|
|
*/
|
|
void Config_Set(Config_t *cfg) {
|
|
osKernelLock();
|
|
BSP_Flash_EraseSector(11);
|
|
BSP_Flash_WriteBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
|
|
osKernelUnlock();
|
|
}
|