336 lines
6.5 KiB
C
336 lines
6.5 KiB
C
#include "config.h"
|
||
#include "flash.h"
|
||
#include "string.h"
|
||
#define DEBUG
|
||
|
||
#define CONFIG_BASE_ADDRESS (ADDR_FLASH_SECTOR_11)
|
||
|
||
#ifdef DEBUG
|
||
|
||
ConfigParam_t param_chassis ={
|
||
|
||
#else
|
||
|
||
static const ConfigParam_t param_chassis ={
|
||
|
||
#endif
|
||
|
||
.chassis = {/**/
|
||
.C6020Angle_param = {
|
||
.p = 30.0f,
|
||
.i = 0.05f,
|
||
.d = 2.0f,
|
||
.i_limit = 20.0f,
|
||
.out_limit = 5000.0f,
|
||
},
|
||
.C6020Omega_param = {
|
||
.p = 45.0f,
|
||
.i = 0.4f,
|
||
.d = 6.0f,
|
||
.i_limit = 2000.0f,
|
||
.out_limit = 25000.0f
|
||
},
|
||
.Chassis_AngleAdjust_param={
|
||
.p = 10.0f,
|
||
.i = 0.0f,
|
||
.d = 0.0f,
|
||
.i_limit = 0.0f,
|
||
.out_limit =6000.0f,
|
||
},
|
||
// 家里雷达pid
|
||
// .RadarAngle_param = {
|
||
// .p = 22.0f,
|
||
// .i = 20.0f,
|
||
// .d = 800.0f,
|
||
// .i_limit = 100.0f,
|
||
// .out_limit =2000.0f,
|
||
// },
|
||
// .RadarSpeed_param = {
|
||
// .p = 7.0f,
|
||
// .i = 0.1f,
|
||
// .d = 50.0f,
|
||
// .i_limit = 1000.0f,
|
||
// .out_limit =8000.0f,
|
||
// },
|
||
|
||
//备馆雷达pid
|
||
// .RadarAngle_param = {
|
||
// .p = 20.0f,
|
||
// .i = 0.06f,
|
||
// .d = 650.0f,
|
||
// .i_limit = 30.0f,
|
||
// .out_limit =2000.0f,
|
||
// },
|
||
// .RadarSpeed_param = {
|
||
// .p = 7.0f,
|
||
// .i = 0.1f,
|
||
// .d = 200.0f,
|
||
// .i_limit = 1000.0f,
|
||
// .out_limit =8000.0f,
|
||
// },
|
||
|
||
//第一次调
|
||
// .RadarAngle_param = {
|
||
// .p = 22.0f,
|
||
// .i = 1.2f,
|
||
// .d = 700.0f,
|
||
// .i_limit = 30.0f,
|
||
// .out_limit =4000.0f,
|
||
// },
|
||
// .RadarSpeed_param = {
|
||
// .p = 9.0f,
|
||
// .i = 0.15f,
|
||
// .d = 450.0f,
|
||
// .i_limit = 1200.0f,
|
||
// .out_limit =8000.0f,
|
||
// },
|
||
|
||
//第二次调
|
||
// .RadarAngle_param = {
|
||
// .p = 30.0f,
|
||
// .i = 0.9f,
|
||
// .d = 650.0f,
|
||
// .i_limit = 30.0f,
|
||
// .out_limit =4000.0f,
|
||
// },
|
||
// .RadarSpeed_param = {
|
||
// .p = 7.8f,
|
||
// .i = 0.3f,
|
||
// .d = 250.0f,
|
||
// .i_limit = 15.0f,
|
||
// .out_limit =8000.0f,
|
||
// },
|
||
|
||
//第三次调
|
||
.RadarAngle_param = {
|
||
.p = 23.0f,
|
||
.i = 0.09f,
|
||
.d = 75.0f,
|
||
.i_limit = 30.0f,
|
||
.out_limit =8000.0f,
|
||
},
|
||
.RadarSpeed_param = {
|
||
.p = 8.3f,
|
||
.i = 0.005f,
|
||
.d = 900.0f,
|
||
.i_limit = 10.0f,
|
||
.out_limit =8000.0f,
|
||
},
|
||
|
||
.SickVx_param = {
|
||
.p =12.0f,
|
||
.i =0.3f,
|
||
.d =1000.0f,
|
||
.i_limit =2500.0f,
|
||
.out_limit =15000.0f,
|
||
},
|
||
.SickVy_param = {
|
||
.p =15.0f,
|
||
.i =0.3f,
|
||
.d =1200.0f,
|
||
.i_limit =2500.0f,
|
||
.out_limit =15000.0f,
|
||
},
|
||
.SickVw_param = {
|
||
.p =20.0f,
|
||
.i =0.3f,
|
||
.d =800.0f,
|
||
.i_limit = 2500.0f,
|
||
.out_limit = 8000.0f
|
||
},
|
||
|
||
.M3508_param = {
|
||
.p = 15.0f,
|
||
.i = 0.0f,
|
||
.d = 1.5f,
|
||
.i_limit = 0.0f,
|
||
.out_limit = 5000.0f,
|
||
},
|
||
// .C6020pitAngle_param = {
|
||
// .p = 10.0f,
|
||
// .i = 0.025f,
|
||
// .d =5.0f,
|
||
// .i_limit = 2000.0f,
|
||
// .out_limit = 3000.0f,
|
||
// },
|
||
// .C6020pitOmega_param = {
|
||
// .p =10.0f,
|
||
// .i =0.0f,
|
||
// .d =0.0f,
|
||
// .i_limit = 2000.0f,
|
||
// .out_limit = 3000.0f,
|
||
// },
|
||
},
|
||
|
||
/*码盘导航*/
|
||
|
||
.ops={
|
||
|
||
.ops_pid={
|
||
/*路径速度*/
|
||
//不要盲目的为了提高速度直接给大out，一定要慢慢往上加并根据情况调整pid参数
|
||
|
||
/*原本的pid，速度不快，很稳定，适合作为基础速度*/
|
||
.path_speed_pid_param=
|
||
{
|
||
.p = 20.0f,
|
||
.i = 0.02f,
|
||
.d = 3.2f,
|
||
.i_limit = 200.0f,
|
||
.out_limit =6000.0f,
|
||
},
|
||
.pid_PosSpeed_xy_param={
|
||
.p =25.0f,
|
||
.i =0.1f,
|
||
.d =0.0f,
|
||
.i_limit =5.0f,
|
||
.out_limit =6000.0f,
|
||
},
|
||
// //比赛pid
|
||
// .path_speed_pid_param=
|
||
// {
|
||
// .p = 0.5f,
|
||
// .i = 0.0f,
|
||
// .d = 0.0f,
|
||
// .i_limit = 0.0f,
|
||
// .out_limit =1000.0f,
|
||
// },
|
||
//
|
||
// /*远距离xy纠正*/
|
||
// .pid_PosSpeed_xy_param={
|
||
// .p =25.0f,
|
||
// .i =0.1f,
|
||
// .d =500.0f,
|
||
// .i_limit =500.0f,
|
||
// .out_limit =20000.0f,
|
||
// },
|
||
/*远距离角度纠正*/
|
||
.pid_OutAngle_param={
|
||
.p = 20.0f,
|
||
.i = 0.0f,
|
||
.d = 0.0f,
|
||
.i_limit = 0.0f,
|
||
.out_limit =2000.0f,
|
||
},
|
||
.pid_InnerAngle_param={
|
||
.p = 20.0f,
|
||
.i = 0.04f,
|
||
.d = 0.0f,
|
||
.i_limit = 0.0f,
|
||
.out_limit =16000.0f,
|
||
},
|
||
|
||
/*近距离xy纠正*/
|
||
.pid_pos_xy_outer_param={
|
||
.p =4.0f,
|
||
.i =0.1f,
|
||
.d =0.5f,
|
||
.i_limit =5.0f,
|
||
.out_limit =1000.0f,
|
||
},
|
||
.pid_pos_xy_inner_param={
|
||
.p =4.0f,
|
||
.i =0.1f,
|
||
.d =0.5f,
|
||
.i_limit =5.0f,
|
||
.out_limit =6000.0f,
|
||
},
|
||
/*近距离角度纠正*/
|
||
//原本的pid
|
||
// .pid_OutAngle_hold_param={
|
||
// .p = 50.0f,
|
||
// .i = 0.3f,
|
||
// .d = 3.5f,
|
||
// .i_limit = 100.0f,
|
||
// .out_limit =1000.0f,
|
||
// },
|
||
// .pid_InnerAngle_hold_param={
|
||
// .p = 30.0f,
|
||
// .i = 0.0f,
|
||
// .d = 0.5f,
|
||
// .i_limit = 0.0f,
|
||
// .out_limit =7000.0f,
|
||
// },
|
||
//比赛pid
|
||
.pid_OutAngle_hold_param={
|
||
.p = 25.0f,
|
||
.i = 0.1f,
|
||
.d = 5.0f,
|
||
.i_limit = 100.0f,
|
||
.out_limit =1000.0f,
|
||
},
|
||
.pid_InnerAngle_hold_param={
|
||
.p = 20.0f,
|
||
.i = 0.1f,
|
||
.d = 0.5f,
|
||
.i_limit = 400.0f,
|
||
.out_limit =7000.0f,
|
||
},
|
||
|
||
/*sick校准pid*/
|
||
.pid_sick_out_param={
|
||
.p =8.0f,
|
||
.i =0.1f,
|
||
.d =0.5f,
|
||
.i_limit =5.0f,
|
||
.out_limit =4000.0f,
|
||
},
|
||
.pid_sick_inner_param={
|
||
.p =5.0f,
|
||
.i =0.1f,
|
||
.d =0.5f,
|
||
.i_limit =5.0f,
|
||
.out_limit =4000.0f,
|
||
},
|
||
},
|
||
|
||
},
|
||
.can = {
|
||
.chassis5065 = BSP_CAN_2,
|
||
.chassis6020 = BSP_CAN_1,
|
||
|
||
},
|
||
};
|
||
|
||
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();
|
||
}
|