/*
 * 配置相关
 */

#include "config.h"

#include <stdint.h>
#include <string.h>

#include "bsp/flash.h"

#define CONFIG_BASE_ADDRESS (ADDR_FLASH_SECTOR_11)

/* clang-format off */
#ifdef DEBUG
Config_RobotParam_t param_default = {
#else
static const Config_RobotParam_t param_default = {
#endif
  .model = ROBOT_MODEL_INFANTRY,

  .chassis = { /* 底盘模块参数 */
    .type = CHASSIS_TYPE_MECANUM,

    .motor_pid_param = {
      .k = 0.001f,
      .p = 1.0f,
      .i = 0.0f,
      .d = 0.0f,
      .i_limit = 1.0f,
      .out_limit = 1.0f,
      .d_cutoff_freq = -1.0f,
      .range = -1.0f,
    },

    .follow_pid_param = {
      .k = 0.5f,
      .p = 1.0f,
      .i = 0.0f,
      .d = 0.0f,
      .i_limit = 1.0f,
      .out_limit = 1.0f,
      .d_cutoff_freq = -1.0f,
      .range = M_2PI,
    },

    .low_pass_cutoff_freq = {
      .in = -1.0f,
      .out = -1.0f,
    },
		
		.reverse = {
			.yaw = false,
		},
		
  }, /* chassis */

  .gimbal = { /* 云台模块参数 */
    .pid = {
      {
//        /* GIMBAL_PID_YAW_OMEGA_IDX */
//        .k = 0.25f,
//        .p = 1.0f,
//        .i = 1.0f,
//        .d = 0.0f,
//        .i_limit = 1.0f,
//        .out_limit = 1.0f,
//        .d_cutoff_freq = -1.0f,
//        .range = -1.0f,
//      }, {
//        /* GIMBAL_PID_YAW_ANGLE_IDX */
//        .k = 12.0f,
//        .p = 1.0f,
//        .i = 0.0f,
//        .d = 0.05f,
//        .i_limit = 0.0f,
//        .out_limit = 10.0f,
//        .d_cutoff_freq = -1.0f,
//        .range = M_2PI,
				/* GIMBAL_PID_YAW_OMEGA_IDX */
        .k = 0.24f,
        .p = 1.0f,
        .i = 0.5f,
        .d = 0.0f,
        .i_limit = 1.0f,
        .out_limit = 1.0f,
        .d_cutoff_freq = -1.0f,
        .range = -1.0f,
      }, {
        /* GIMBAL_PID_YAW_ANGLE_IDX */
        .k = 10.0f,
        .p = 1.0f,
        .i = 0.0f,
        .d = 0.05f,
        .i_limit = 0.0f,
        .out_limit = 10.0f,
        .d_cutoff_freq = -1.0f,
        .range = M_2PI,
      }, {
//        /* GIMBAL_PID_PIT_OMEGA_IDX */
//        .k = 0.35f,
//        .p = 1.0f,
//        .i = 0.f,
//        .d = 0.003f,
//        .i_limit = 1.0f,
//        .out_limit = 1.0f,
//        .d_cutoff_freq = -1.0f,
//        .range = -1.0f,
//      }, {
//        /* GIMBAL_PID_PIT_ANGLE_IDX */
//        .k = 15.0f,
//        .p = 1.0f,
//        .i = 0.0f,
//        .d = 0.0f,
//        .i_limit = 0.0f,
//        .out_limit = 10.0f,
//        .d_cutoff_freq = -1.0f,
//        .range = M_2PI,
				/* GIMBAL_PID_PIT_OMEGA_IDX */
        .k = 0.25f,
        .p = 1.0f,
        .i = 0.0f,
        .d = 0.0f,
        .i_limit = 1.0f,
        .out_limit = 1.0f,
        .d_cutoff_freq = -1.0f,
        .range = -1.0f,
      }, {
        /* GIMBAL_PID_PIT_ANGLE_IDX */
        .k = 2.0f,
        .p = 5.0f,
        .i = 2.5f,
        .d = 0.0f,
        .i_limit = 0.0f,
        .out_limit = 10.0f,
        .d_cutoff_freq = -1.0f,
        .range = M_2PI,
      },
    }, /* pid */

    .pitch_travel_rad = 1.05f,
    
    .low_pass_cutoff_freq = {
      .out = -1.0f,
      .gyro = 1000.0f,
    },

    .reverse = {
      .yaw = false,
      .pit = true,
    },

    .pit_ctrl_reverse = false,
  }, /* gimbal */

  .shoot = { /* 射击模块参数 */
            
    .fric_pid_param = {
      .k = 0.001f,
      .p = 1.0f,
      .i = 0.2f,
      .d = 0.01f,
      .i_limit = 0.5f,
      .out_limit = 0.5f,
      .d_cutoff_freq = -1.0f,
    },
    .trig_pid_param = {
      .k = 12.0f,
      .p = 1.0f,
      .i = 0.0f,
      .d = 0.0450000018f,
      .i_limit = 1.0f,
      .out_limit = 1.0f,
      .d_cutoff_freq = -1.0f,
      .range = M_2PI,
    },
    .low_pass_cutoff_freq = {
      .in = {
        .fric = -1.0f,
        .trig = -1.0f,
      },
      .out = {
        .fric = -1.0f,
        .trig = -1.0f,
      },
    },
    .num_trig_tooth = 10.0f,
    .trig_gear_ratio = 36.0f,
    .fric_radius = 0.03f,
    .cover_open_duty = 0.10f,
    .cover_close_duty = 0.050f,
    .model = SHOOT_MODEL_17MM,
    .bullet_speed = 30.f,
    .min_shoot_delay = (uint32_t)(1000.0f / 10.0f),
  }, /* shoot */

  .can = {
    .chassis = BSP_CAN_1,
    .gimbal = BSP_CAN_2,
    .shoot = BSP_CAN_2,      
    .cap = BSP_CAN_1,
    }, /* can */
}; /* param_default */

#ifdef DEBUG
Config_RobotParam_t param_hero = {
#else
static const Config_RobotParam_t param_hero = {
#endif
  .model = ROBOT_MODEL_HERO,

  .chassis = { /* 底盘模块参数 */
    .type = CHASSIS_TYPE_MECANUM,

    .motor_pid_param = {
      .k = 0.0011f,
      .p = 1.0f,
      .i = 0.001f,
      .d = 0.0f,
      .i_limit = 1.0f,
      .out_limit = 1.0f,
      .d_cutoff_freq = -1.0f,
      .range = -1.0f,
    },

    .follow_pid_param = {
      .k = 0.5f,
      .p = 1.0f,
      .i = 0.0f,
      .d = 0.0f,
      .i_limit = 1.0f,
      .out_limit = 1.0f,
      .d_cutoff_freq = -1.0f,
      .range = M_2PI,
    },

    .low_pass_cutoff_freq = {
      .in = -1.0f,
      .out = -1.0f,
    },
		
		.reverse = {
			.yaw = false,
		},
  }, /* chassis */

  .gimbal = { /* 云台模块参数 */
    .pid = {
      {
        /* GIMBAL_PID_YAW_OMEGA_IDX */
        .k = 0.45f,
        .p = 1.0f,
        .i = 6.0f,
        .d = 0.0008f,
        .i_limit = 1.0f,
        .out_limit = 1.0f,
        .d_cutoff_freq = -1.0f,
        .range = -1.0f,
      }, {
        /* GIMBAL_PID_YAW_ANGLE_IDX */
        .k = 20.0f,
        .p = 1.0f,
        .i = 0.0f,
        .d = 0.0f,
        .i_limit = 0.0f,
        .out_limit = 10.0f,
        .d_cutoff_freq = -1.0f,
        .range = M_2PI,
      }, {
        /* GIMBAL_PID_PIT_OMEGA_IDX */
        .k = 0.25f,
        .p = 1.0f,
        .i = 0.0f,
        .d = 0.0f,
        .i_limit = 1.0f,
        .out_limit = 1.0f,
        .d_cutoff_freq = -1.0f,
        .range = -1.0f,
      }, {
        /* GIMBAL_PID_PIT_ANGLE_IDX */
        .k = 12.0f,
        .p = 1.0f,
        .i = 0.0f,
        .d = 0.0f,
        .i_limit = 0.0f,
        .out_limit = 10.0f,
        .d_cutoff_freq = -1.0f,
        .range = M_2PI,
      },
    }, /* pid */

		.pitch_travel_rad = 1.07685447f,
    
    .low_pass_cutoff_freq = {
      .out = -1.0f,
      .gyro = 1000.0f,
    },

    .reverse = {
      .yaw = false,
      .pit = false,
    },

    .pit_ctrl_reverse = false,
  }, /* gimbal */

  .shoot = { /* 射击模块参数 */
            
    .fric_pid_param = {
      .k = 0.001f,
      .p = 1.0f,
      .i = 0.2f,
      .d = 0.01f,
      .i_limit = 0.5f,
      .out_limit = 0.5f,
      .d_cutoff_freq = -1.0f,
    },
    .trig_pid_param = {
      .k = 10.0f,
      .p = 1.0f,
      .i = 0.0f,
      .d = 0.032f,
      .i_limit = 1.0f,
      .out_limit = 1.0f,
      .d_cutoff_freq = -1.0f,
      .range = M_2PI,
    },
    .low_pass_cutoff_freq = {
      .in = {
        .fric = -1.0f,
        .trig = -1.0f,
      },
      .out = {
        .fric = -1.0f,
        .trig = -1.0f,
      },
    },
    .num_trig_tooth = 5.0f,
    .trig_gear_ratio = 19.0f,
    .fric_radius = 0.03f,
    .cover_open_duty = 0.125f,
    .cover_close_duty = 0.075f,
    .model = SHOOT_MODEL_42MM,
    .bullet_speed = 16.0f,
    .min_shoot_delay = (uint32_t)(1000.0f / 10.0f),
  }, /* shoot */

  .can = {
    .chassis = BSP_CAN_1,
    .gimbal = BSP_CAN_2,
    .shoot = BSP_CAN_2,      
    .cap = BSP_CAN_1,
    }, /* can */
}; /* param_hero */      

#ifdef DEBUG
Config_RobotParam_t param_sentry_chassis = {
#else
static const Config_RobotParam_t param_sentry_chassis = {
#endif
  .model = ROBOT_MODEL_SENTRY_CHASSIS,

  .chassis = { /* 底盘模块参数 */
    .type = CHASSIS_TYPE_MECANUM,

    .motor_pid_param = {
      .k = 0.001f,
      .p = 1.0f,
      .i = 0.0f,
      .d = 0.0f,
      .i_limit = 1.0f,
      .out_limit = 1.0f,
      .d_cutoff_freq = -1.0f,
      .range = -1.0f,
    },

    .follow_pid_param = {
      .k = 0.5f,
      .p = 1.0f,
      .i = 0.0f,
      .d = 0.0f,
      .i_limit = 1.0f,
      .out_limit = 1.0f,
      .d_cutoff_freq = -1.0f,
      .range = M_2PI,
    },

    .low_pass_cutoff_freq = {
      .in = -1.0f,
      .out = -1.0f,
    },
		
		.reverse = {
			.yaw = false,
		},
		
  }, /* chassis */

  .gimbal = { /* 云台模块参数 */
    .pid = {
      {
//        /* GIMBAL_PID_YAW_OMEGA_IDX */
//        .k = 0.25f,
//        .p = 1.0f,
//        .i = 1.0f,
//        .d = 0.0f,
//        .i_limit = 1.0f,
//        .out_limit = 1.0f,
//        .d_cutoff_freq = -1.0f,
//        .range = -1.0f,
//      }, {
//        /* GIMBAL_PID_YAW_ANGLE_IDX */
//        .k = 12.0f,
//        .p = 1.0f,
//        .i = 0.0f,
//        .d = 0.05f,
//        .i_limit = 0.0f,
//        .out_limit = 10.0f,
//        .d_cutoff_freq = -1.0f,
//        .range = M_2PI,
				/* GIMBAL_PID_YAW_OMEGA_IDX */
        .k = 0.24f,
        .p = 1.0f,
        .i = 0.5f,
        .d = 0.0f,
        .i_limit = 1.0f,
        .out_limit = 1.0f,
        .d_cutoff_freq = -1.0f,
        .range = -1.0f,
      }, {
        /* GIMBAL_PID_YAW_ANGLE_IDX */
        .k = 10.0f,
        .p = 1.0f,
        .i = 0.0f,
        .d = 0.05f,
        .i_limit = 0.0f,
        .out_limit = 10.0f,
        .d_cutoff_freq = -1.0f,
        .range = M_2PI,
      }, {
//        /* GIMBAL_PID_PIT_OMEGA_IDX */
//        .k = 0.35f,
//        .p = 1.0f,
//        .i = 0.f,
//        .d = 0.003f,
//        .i_limit = 1.0f,
//        .out_limit = 1.0f,
//        .d_cutoff_freq = -1.0f,
//        .range = -1.0f,
//      }, {
//        /* GIMBAL_PID_PIT_ANGLE_IDX */
//        .k = 15.0f,
//        .p = 1.0f,
//        .i = 0.0f,
//        .d = 0.0f,
//        .i_limit = 0.0f,
//        .out_limit = 10.0f,
//        .d_cutoff_freq = -1.0f,
//        .range = M_2PI,
				/* GIMBAL_PID_PIT_OMEGA_IDX */
        .k = 0.25f,
        .p = 1.0f,
        .i = 0.0f,
        .d = 0.0f,
        .i_limit = 1.0f,
        .out_limit = 1.0f,
        .d_cutoff_freq = -1.0f,
        .range = -1.0f,
      }, {
        /* GIMBAL_PID_PIT_ANGLE_IDX */
        .k = 2.0f,
        .p = 5.0f,
        .i = 2.5f,
        .d = 0.0f,
        .i_limit = 0.0f,
        .out_limit = 10.0f,
        .d_cutoff_freq = -1.0f,
        .range = M_2PI,
      },
    }, /* pid */

    .pitch_travel_rad = 0.85f,
    
    .low_pass_cutoff_freq = {
      .out = -1.0f,
      .gyro = 1000.0f,
    },

    .reverse = {
      .yaw = false,
      .pit = true,
    },

    .pit_ctrl_reverse = true,
  }, /* gimbal */

  .shoot = { /* 射击模块参数 */
            
    .fric_pid_param = {
      .k = 0.001f,
      .p = 1.0f,
      .i = 0.2f,
      .d = 0.01f,
      .i_limit = 0.5f,
      .out_limit = 0.5f,
      .d_cutoff_freq = -1.0f,
    },
    .trig_pid_param = {
      .k = 12.0f,
      .p = 1.0f,
      .i = 0.0f,
      .d = 0.0450000018f,
      .i_limit = 1.0f,
      .out_limit = 1.0f,
      .d_cutoff_freq = -1.0f,
      .range = M_2PI,
    },
    .low_pass_cutoff_freq = {
      .in = {
        .fric = -1.0f,
        .trig = -1.0f,
      },
      .out = {
        .fric = -1.0f,
        .trig = -1.0f,
      },
    },
    .num_trig_tooth = 8.0f,
    .trig_gear_ratio = 36.0f,
    .fric_radius = 0.03f,
    .cover_open_duty = 0.10f,
    .cover_close_duty = 0.050f,
    .model = SHOOT_MODEL_17MM,
    .bullet_speed = 6.f,
    .min_shoot_delay = (uint32_t)(1000.0f / 10.0f),
  }, /* shoot */

  .can = {
    .chassis = BSP_CAN_1,
    .gimbal = BSP_CAN_2,
    .shoot = BSP_CAN_2,      
    .cap = BSP_CAN_1,
    }, /* can */
}; /* param_sentry_chassis */

#ifdef DEBUG
Config_RobotParam_t param_sentry_gimbal = {
#else
static const Config_RobotParam_t param_sentry_gimbal = {
#endif
  .model = ROBOT_MODEL_SENTRY_GIMBAL,

  .chassis = { /* 底盘模块参数 */
    .type = CHASSIS_TYPE_MECANUM,

    .motor_pid_param = {
      .k = 0.001f,
      .p = 1.0f,
      .i = 0.0f,
      .d = 0.0f,
      .i_limit = 1.0f,
      .out_limit = 1.0f,
      .d_cutoff_freq = -1.0f,
      .range = -1.0f,
    },

    .follow_pid_param = {
      .k = 0.5f,
      .p = 1.0f,
      .i = 0.0f,
      .d = 0.0f,
      .i_limit = 1.0f,
      .out_limit = 1.0f,
      .d_cutoff_freq = -1.0f,
      .range = M_2PI,
    },

    .low_pass_cutoff_freq = {
      .in = -1.0f,
      .out = -1.0f,
    },
		
		.reverse = {
			.yaw = false,
		},
		
  }, /* chassis */

  .gimbal = { /* 云台模块参数 */
    .pid = {
      {
//        /* GIMBAL_PID_YAW_OMEGA_IDX */
//        .k = 0.25f,
//        .p = 1.0f,
//        .i = 1.0f,
//        .d = 0.0f,
//        .i_limit = 1.0f,
//        .out_limit = 1.0f,
//        .d_cutoff_freq = -1.0f,
//        .range = -1.0f,
//      }, {
//        /* GIMBAL_PID_YAW_ANGLE_IDX */
//        .k = 12.0f,
//        .p = 1.0f,
//        .i = 0.0f,
//        .d = 0.05f,
//        .i_limit = 0.0f,
//        .out_limit = 10.0f,
//        .d_cutoff_freq = -1.0f,
//        .range = M_2PI,
				/* GIMBAL_PID_YAW_OMEGA_IDX */
        .k = 0.24f,
        .p = 1.0f,
        .i = 0.5f,
        .d = 0.0f,
        .i_limit = 1.0f,
        .out_limit = 1.0f,
        .d_cutoff_freq = -1.0f,
        .range = -1.0f,
      }, {
        /* GIMBAL_PID_YAW_ANGLE_IDX */
        .k = 10.0f,
        .p = 1.0f,
        .i = 0.0f,
        .d = 0.05f,
        .i_limit = 0.0f,
        .out_limit = 10.0f,
        .d_cutoff_freq = -1.0f,
        .range = M_2PI,
      }, {
//        /* GIMBAL_PID_PIT_OMEGA_IDX */
//        .k = 0.35f,
//        .p = 1.0f,
//        .i = 0.f,
//        .d = 0.003f,
//        .i_limit = 1.0f,
//        .out_limit = 1.0f,
//        .d_cutoff_freq = -1.0f,
//        .range = -1.0f,
//      }, {
//        /* GIMBAL_PID_PIT_ANGLE_IDX */
//        .k = 15.0f,
//        .p = 1.0f,
//        .i = 0.0f,
//        .d = 0.0f,
//        .i_limit = 0.0f,
//        .out_limit = 10.0f,
//        .d_cutoff_freq = -1.0f,
//        .range = M_2PI,
				/* GIMBAL_PID_PIT_OMEGA_IDX */
        .k = 0.25f,
        .p = 1.0f,
        .i = 0.0f,
        .d = 0.0f,
        .i_limit = 1.0f,
        .out_limit = 1.0f,
        .d_cutoff_freq = -1.0f,
        .range = -1.0f,
      }, {
        /* GIMBAL_PID_PIT_ANGLE_IDX */
        .k = 2.0f,
        .p = 5.0f,
        .i = 2.5f,
        .d = 0.0f,
        .i_limit = 0.0f,
        .out_limit = 10.0f,
        .d_cutoff_freq = -1.0f,
        .range = M_2PI,
      },
    }, /* pid */

    .pitch_travel_rad = 0.85f,
    
    .low_pass_cutoff_freq = {
      .out = -1.0f,
      .gyro = 1000.0f,
    },

    .reverse = {
      .yaw = false,
      .pit = true,
    },

    .pit_ctrl_reverse = true,
  }, /* gimbal */

  .shoot = { /* 射击模块参数 */
            
    .fric_pid_param = {
      .k = 0.001f,
      .p = 1.0f,
      .i = 0.2f,
      .d = 0.01f,
      .i_limit = 0.5f,
      .out_limit = 0.5f,
      .d_cutoff_freq = -1.0f,
    },
    .trig_pid_param = {
      .k = 12.0f,
      .p = 1.0f,
      .i = 0.0f,
      .d = 0.0450000018f,
      .i_limit = 1.0f,
      .out_limit = 1.0f,
      .d_cutoff_freq = -1.0f,
      .range = M_2PI,
    },
    .low_pass_cutoff_freq = {
      .in = {
        .fric = -1.0f,
        .trig = -1.0f,
      },
      .out = {
        .fric = -1.0f,
        .trig = -1.0f,
      },
    },
    .num_trig_tooth = 8.0f,
    .trig_gear_ratio = 36.0f,
    .fric_radius = 0.03f,
    .cover_open_duty = 0.10f,
    .cover_close_duty = 0.050f,
    .model = SHOOT_MODEL_17MM,
    .bullet_speed = 25.f,
    .min_shoot_delay = (uint32_t)(1000.0f / 20.0f),
  }, /* shoot */

  .can = {
    .chassis = BSP_CAN_1,
    .gimbal = BSP_CAN_2,
    .shoot = BSP_CAN_2,      
    .cap = BSP_CAN_1,
    }, /* can */
}; /* param_sentry_gimbal */

/* static const Config_RobotParam_t param_xxx; */

static const Config_PilotCfg_t cfg_qs = {
  .param = {
		.sens_mouse = 0.06f,
		.sens_rc = 6.0f,
    .map = {
      .key_map[CMD_BEHAVIOR_FORE] = {CMD_ACTIVE_PRESSED, CMD_KEY_W},
      .key_map[CMD_BEHAVIOR_BACK] = {CMD_ACTIVE_PRESSED, CMD_KEY_S},
      .key_map[CMD_BEHAVIOR_LEFT] = {CMD_ACTIVE_PRESSED, CMD_KEY_A},
      .key_map[CMD_BEHAVIOR_RIGHT] = {CMD_ACTIVE_PRESSED, CMD_KEY_D},
      .key_map[CMD_BEHAVIOR_ACCELERATE] = {CMD_ACTIVE_PRESSED, CMD_KEY_SHIFT},
      .key_map[CMD_BEHAVIOR_DECELEBRATE] = {CMD_ACTIVE_PRESSED, CMD_KEY_CTRL},
      .key_map[CMD_BEHAVIOR_FIRE] = {CMD_ACTIVE_PRESSED, CMD_L_CLICK},
      .key_map[CMD_BEHAVIOR_FIRE_MODE] = {CMD_ACTIVE_PRESSING, CMD_R_CLICK},
      .key_map[CMD_BEHAVIOR_FOLLOWGIMBAL35] = {CMD_ACTIVE_PRESSING, CMD_KEY_E},
      .key_map[CMD_BEHAVIOR_OPENCOVER] =  {CMD_ACTIVE_PRESSING, CMD_KEY_F},
      .key_map[CMD_BEHAVIOR_REVTRIG] = {CMD_ACTIVE_PRESSING, CMD_KEY_R},
      .key_map[CMD_BEHAVIOR_ROTOR] = {CMD_ACTIVE_PRESSING, CMD_KEY_G},
    },
    .move = {
      .move_sense = 1.6f,
      .move_fast_sense = 2.4f,
      .move_slow_sense = 0.8f,		
    },
    .screen = {
      .height = 1080,
      .width = 1920,
    },
  },
};

static const Config_PilotCfg_t cfg_zyma = {
  .param = {
		.sens_mouse = 0.06f,
		.sens_rc = 6.0f,
    .map = {
      .key_map[CMD_BEHAVIOR_FORE] = {CMD_ACTIVE_PRESSED, CMD_KEY_W},
      .key_map[CMD_BEHAVIOR_BACK] = {CMD_ACTIVE_PRESSED, CMD_KEY_S},
      .key_map[CMD_BEHAVIOR_LEFT] = {CMD_ACTIVE_PRESSED, CMD_KEY_A},
      .key_map[CMD_BEHAVIOR_RIGHT] = {CMD_ACTIVE_PRESSED, CMD_KEY_D},
      .key_map[CMD_BEHAVIOR_ACCELERATE] = {CMD_ACTIVE_PRESSED, CMD_KEY_SHIFT},
      .key_map[CMD_BEHAVIOR_DECELEBRATE] = {CMD_ACTIVE_PRESSED, CMD_KEY_CTRL},
      .key_map[CMD_BEHAVIOR_FIRE] = {CMD_ACTIVE_PRESSED, CMD_L_CLICK},
      .key_map[CMD_BEHAVIOR_FIRE_MODE] = {CMD_ACTIVE_PRESSING, CMD_R_CLICK},
      .key_map[CMD_BEHAVIOR_FOLLOWGIMBAL35] = {CMD_ACTIVE_PRESSING, CMD_KEY_E},
      .key_map[CMD_BEHAVIOR_OPENCOVER] =  {CMD_ACTIVE_PRESSING, CMD_KEY_F},
      .key_map[CMD_BEHAVIOR_REVTRIG] = {CMD_ACTIVE_PRESSING, CMD_KEY_R},
      .key_map[CMD_BEHAVIOR_ROTOR] = {CMD_ACTIVE_PRESSING, CMD_KEY_G},
    },
    .move = {
      .move_sense = 1.6f,
      .move_fast_sense = 2.4f,
      .move_slow_sense = 1.6f,		
    },       
  },
};

/* static const Config_PilotCfg_t cfg_xx; */

/* clang-format on */

static const Config_RobotParamMap_t robot_param_map[] = {
    {"default", &param_default},
    {"infantry", &param_default},
    {"hero", &param_hero},
    {"sentry_chassis", &param_sentry_chassis},
    {"sentry_gimbal", &param_sentry_gimbal},
    // {"engineer", &param_engineer},
    // {"drone", &param_drone},
    // {"sentry", &param_sentry},
    /* {"xxx", &param_xxx}, */
    {NULL, NULL},
};

static const Config_PilotCfgMap_t pilot_cfg_map[] = {
    {"qs", &cfg_qs},
    {"zyma", &cfg_zyma},
    /* {"xx", &cfg_xx}, */
    {NULL, NULL},
};

/**
 * \brief 从Flash读取配置信息
 *
 * \param cfg 配置信息
 */
void Config_Get(Config_t *cfg) {
  BSP_Flash_ReadBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
  cfg->pilot_cfg = Config_GetPilotCfg(cfg->pilot_cfg_name);
  cfg->robot_param = Config_GetRobotParam(cfg->robot_param_name);
  /* 防止第一次烧写后访问NULL指针 */
  if (cfg->robot_param == NULL) cfg->robot_param = &param_default;
  if (cfg->pilot_cfg == NULL) cfg->pilot_cfg = &cfg_qs;
  /* 防止擦除后全为1 */
  if ((uint32_t)(cfg->robot_param) == UINT32_MAX)
    cfg->robot_param = &param_default;
  if ((uint32_t)(cfg->pilot_cfg) == UINT32_MAX) cfg->pilot_cfg = &cfg_qs;
}

/**
 * \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();
}

/**
 * @brief 通过机器人参数名称获取机器人参数的指针
 *
 * @param robot_param_name 机器人参数名称
 * @return const Config_RobotParam_t* 机器人参数的指针
 */
const Config_RobotParam_t *Config_GetRobotParam(const char *robot_param_name) {
  if (robot_param_name == NULL) return NULL;
  for (size_t j = 0; robot_param_map[j].name != NULL; j++) {
    if (strcmp(robot_param_map[j].name, robot_param_name) == 0) {
      return robot_param_map[j].param;
    }
  }
  return NULL; /* No match. */
}

/**
 * @brief 通过操作手配置名称获取操作手配置的指针
 *
 * @param pilot_cfg_name 操作手配置名称
 * @return const Config_PilotCfg_t* 操作手配置的指针
 */
const Config_PilotCfg_t *Config_GetPilotCfg(const char *pilot_cfg_name) {
  if (pilot_cfg_name == NULL) return NULL;
  for (size_t j = 0; pilot_cfg_map[j].name != NULL; j++) {
    if (strcmp(pilot_cfg_map[j].name, pilot_cfg_name) == 0) {
      return pilot_cfg_map[j].param;
    }
  }
  return NULL; /* No match. */
}

const Config_PilotCfgMap_t *Config_GetPilotNameMap(void) {
  return pilot_cfg_map;
}

const Config_RobotParamMap_t *Config_GetRobotNameMap(void) {
  return robot_param_map;
}