2025-03-12 10:46:02 +08:00
|
|
|
#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_up ={
|
|
|
|
|
|
|
|
//#else
|
|
|
|
//static const ConfigParam_t param_up ={
|
|
|
|
//#endif
|
|
|
|
//
|
|
|
|
//
|
|
|
|
//
|
|
|
|
//
|
|
|
|
//};
|
|
|
|
|
|
|
|
#ifdef DEBUG
|
|
|
|
|
|
|
|
ConfigParam_t param_chassis ={
|
|
|
|
|
|
|
|
#else
|
|
|
|
|
|
|
|
static const ConfigParam_t param_chassis ={
|
|
|
|
|
|
|
|
|
|
|
|
#endif
|
|
|
|
.up={
|
|
|
|
|
2025-03-17 00:32:06 +08:00
|
|
|
.M2006_angle_param = {
|
|
|
|
.p = 25.0f,
|
|
|
|
.i = 0.0f,
|
|
|
|
.d = 1.5f,
|
|
|
|
.i_limit = 1000.0f,
|
2025-03-12 10:46:02 +08:00
|
|
|
.out_limit = 3000.0f,
|
|
|
|
},
|
2025-03-17 00:32:06 +08:00
|
|
|
.M2006_speed_param = {
|
|
|
|
.p = 5.0f,
|
|
|
|
.i = 0.3f,
|
|
|
|
.d = 0.0f,
|
2025-03-12 10:46:02 +08:00
|
|
|
.i_limit = 2000.0f,
|
|
|
|
.out_limit = 3000.0f,
|
|
|
|
},
|
|
|
|
.UP_GM6020_angle_param={
|
|
|
|
.p = 30.0f,
|
|
|
|
.i = 20.0f,
|
|
|
|
.d =0.0f,
|
|
|
|
.i_limit = 200.0f,
|
|
|
|
.out_limit = 1500.0f,
|
|
|
|
} ,
|
|
|
|
.UP_GM6020_speed_param={
|
|
|
|
.p =3.0f,
|
|
|
|
.i =0.5f,
|
|
|
|
.d =0.0f,
|
|
|
|
.i_limit = 200.0f,
|
|
|
|
.out_limit = 3000.0f
|
|
|
|
},
|
|
|
|
.M3508_speed_param={
|
|
|
|
.p = 15.1f,
|
|
|
|
.i = 0.02f,
|
|
|
|
.d = 3.2f,
|
|
|
|
.i_limit = 200.0f,
|
|
|
|
.out_limit =6000.0f,
|
2025-03-17 00:32:06 +08:00
|
|
|
},
|
|
|
|
.go_param={
|
|
|
|
.rev = 0,
|
|
|
|
.T=0.1,
|
|
|
|
.W=0.1,
|
|
|
|
.K_P=0.1,
|
|
|
|
.K_W=0.1,
|
2025-03-12 10:46:02 +08:00
|
|
|
}
|
|
|
|
|
2025-03-17 00:32:06 +08:00
|
|
|
|
2025-03-12 10:46:02 +08:00
|
|
|
},
|
|
|
|
|
2025-03-17 00:32:06 +08:00
|
|
|
// .chassis = {/**/
|
|
|
|
// .C6020pitAngle_param = {
|
|
|
|
// .p = 15.0f,
|
|
|
|
// .i = 0.3f,
|
|
|
|
// .d =0.0f,
|
|
|
|
// .i_limit = 200.0f,
|
|
|
|
// .out_limit = 3000.0f,
|
|
|
|
// },
|
|
|
|
// .C6020pitOmega_param = {
|
|
|
|
// .p =30.0f,
|
|
|
|
// .i =0.3f,
|
|
|
|
// .d =0.0f,
|
|
|
|
// .i_limit = 200.0f,
|
|
|
|
// .out_limit = 3000.0f
|
|
|
|
// },
|
|
|
|
//
|
|
|
|
// .Gimbal_yawAngle_param = {
|
|
|
|
// .p =8.0f,
|
|
|
|
// .i =0.0f,
|
|
|
|
// .d =0.0f,
|
|
|
|
// .i_limit = 200.0f,
|
|
|
|
// .out_limit = 3000.0f
|
|
|
|
// },
|
|
|
|
//
|
|
|
|
// .Gimbal_yawOmega_param = {
|
|
|
|
// .p =18.0f,
|
|
|
|
// .i =0.0f,
|
|
|
|
// .d =0.0f,
|
|
|
|
// .i_limit = 200.0f,
|
|
|
|
// .out_limit = 3000.0f
|
|
|
|
// },
|
|
|
|
//
|
|
|
|
// .Gimbal_pitchAngle_param = {
|
|
|
|
// .p =8.0f,
|
|
|
|
// .i =0.0f,
|
|
|
|
// .d =0.0f,
|
|
|
|
// .i_limit = 200.0f,
|
|
|
|
// .out_limit = 3000.0f
|
|
|
|
// },
|
|
|
|
//
|
|
|
|
// .Gimbal_pitchOmega_param = {
|
|
|
|
// .p =18.0f,
|
|
|
|
// .i =0.0f,
|
|
|
|
// .d =0.0f,
|
|
|
|
// .i_limit = 200.0f,
|
|
|
|
// .out_limit = 3000.0f
|
|
|
|
// },
|
|
|
|
// .AngleCor_param = {
|
|
|
|
// .p =0.8f,
|
|
|
|
// .i =0.0f,
|
|
|
|
// .d =1.0f,
|
|
|
|
// .i_limit = 0.0f,
|
|
|
|
// .out_limit =5000.0f,
|
|
|
|
// },
|
|
|
|
//
|
|
|
|
// .OmegaCor_param = {
|
|
|
|
// .p =23.5f,
|
|
|
|
// .i =0.0f,
|
|
|
|
// .d =0.05f,
|
|
|
|
// .i_limit = 0.0f,
|
|
|
|
// .out_limit =5000.0f,
|
|
|
|
// },
|
|
|
|
//
|
|
|
|
// .ImuCor_param = {
|
|
|
|
// .p =95.0f,
|
|
|
|
// .i =0.0f,
|
|
|
|
// .d =0.0f,
|
|
|
|
// .i_limit = 0.0f,
|
|
|
|
// .out_limit =200.0f,
|
|
|
|
// },
|
|
|
|
//
|
|
|
|
// .DisCamera_param = {
|
|
|
|
// .p =80.0f,
|
|
|
|
// .i =0.1f,
|
|
|
|
// .d =0.0f,
|
|
|
|
// .i_limit = 0.0f,
|
|
|
|
// .out_limit =5000.0f,
|
|
|
|
// },
|
2025-03-12 10:46:02 +08:00
|
|
|
|
2025-03-17 00:32:06 +08:00
|
|
|
// .M3508_param = {
|
|
|
|
// .p = 15.1f,
|
|
|
|
// .i = 0.02f,
|
|
|
|
// .d = 3.2f,
|
|
|
|
// .i_limit = 200.0f,
|
|
|
|
// .out_limit =6000.0f,
|
|
|
|
// }
|
|
|
|
//
|
|
|
|
|
|
|
|
// },
|
2025-03-12 10:46:02 +08:00
|
|
|
|
|
|
|
|
|
|
|
.can = {
|
|
|
|
.pitch6020 = BSP_CAN_1,
|
|
|
|
.motor3508 = BSP_CAN_1,
|
|
|
|
.chassis6020 = BSP_CAN_1,
|
|
|
|
.chassis5065 = BSP_CAN_1,
|
|
|
|
|
|
|
|
.sick = BSP_CAN_2,
|
|
|
|
},
|
|
|
|
|
|
|
|
};
|
|
|
|
const ConfigParam_t *Config_ChassisGet(void)
|
|
|
|
{
|
|
|
|
return ¶m_chassis;
|
|
|
|
}
|
|
|
|
|
|
|
|
///*获取导航地图*/
|
|
|
|
//void set_ops_path(ConfigParam_t *config, const point_t *path, int8_t path_num) {
|
|
|
|
// config->ops.path = path;
|
|
|
|
// config->ops.path_num = path_num;
|
|
|
|
//}
|
|
|
|
/**
|
|
|
|
* \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();
|
|
|
|
}
|