#include "board.h"
#include "soft_system.h"
#include "soft_flash.h"
#include "auto_pilot.h"
#include "bsp_V8M_flash.h"
#include "control_rate.h"
#include "control_throttle.h"
#include "data_save.h"
#include "gcs_vklink_v30.h"
#include "geomatry.h"
#include "my_math.h"
#include "params.h"
#include "pilot_navigation.h"
#include "soft_delay.h"
#include "soft_gs.h"
#include "soft_motor_output.h"
#include "soft_port_uart4.h"
#include "soft_rc_input.h"
#include "soft_time.h"
#include "ver_config.h"
#include <string.h>

#ifdef GD25Q16_FLASH
#include "hard_flash_gd25q16.h"
#else
#include "hard_flash_at45db.h"
#endif
/**
 * @brief flash 接口初始化
 *
 */
void flash_at45db_initial(void) { flash_at45db_init(); }

/**
 * @brief 检查 flash 是否挂载
 *
 * @return true 已挂载
 * @return false 不存在
 */
bool flash_isexist(void)
{
    if (AT45DB_Check() == 0)
        return false;
    else
        return true;
}

/**
 * @brief 检查 flash 是否正在忙
 *
 * @return true 正在忙
 * @return false 可用
 */
bool flash_isbusy(void)
{
    if (AT45DB_IS_BUSY() == 0)
        return true;
    else
        return false;
}

/**
 * @brief flash 读取指定地址的 n bytes 数据
 *
 * @param add 要读取的地址
 * @param pdata 读出数据存放的 buffer 指针
 * @param len 读取的字节数
 */
void flash_read_bytes(unsigned int add, unsigned char *pdata,
                      unsigned short len)
{
    AT45DB_Read_Bytes(add, pdata, len);
}

/**
 * @brief flash 指定地址写入 n bytes 的数据
 *
 * @param add 要写入的地址
 * @param pdata 指向待写入的数据的指针
 * @param len 待写入的字节数
 */
void flash_write_bytes(unsigned int add, unsigned char *pdata,
                       unsigned short len)
{
    AT45DB_Write_Bytes(add, pdata, len);
}

/**
 * @brief flash 读取一个 int16 数据
 *
 * @param add 读取的地址
 * @return short 读取的数据
 */
short flash_read_int16(unsigned int add) { return AT45DB_Read_int16(add); }

/**
 * @brief flash 写入一个 int16 数据
 *
 * @param add 写入的地址
 * @param wvalue 待写入的数据
 */
void flash_write_int16(unsigned int add, short wvalue)
{
    AT45DB_Write_int16(add, wvalue);
}

void flash_read_page(uint16_t Page_Add, uint8_t *pdata)
{
    AT45DB_ReadPage(Page_Add, pdata);
}
void flash_write_page(uint16_t page, uint8_t *Data)
{
    AT45DB_WritePage(page, Data);
}

void flash_write_bytes_ononepage(uint16_t page, uint8_t *pdata, uint16_t len)
{
    AT45DB_WriteBytes_OnOnePage(page, pdata, len);
}

// @brief 装载默认的 PID 参数
void load_default_pidinf(void)
{
    unsigned char i = 0;

    pidinf._pid_flag = (int16_t)0xA12A;

    pidinf._Roll_Angle_P = 450;
    pidinf._Roll_Gyro_P = 60;
    pidinf._Roll_Gyro_I = 5;
    pidinf._Roll_Gyro_D = 5;
    pidinf._Roll_Gyro_Imax = 150;

    pidinf._Pitch_Angle_P = 450;
    pidinf._Pitch_Gyro_P = 60;
    pidinf._Pitch_Gyro_I = 5;
    pidinf._Pitch_Gyro_D = 5;
    pidinf._Pitch_Gyro_Imax = 150;

    pidinf._Yaw_Angle_P = 450;
    pidinf._Yaw_Gyro_P = 350;
    pidinf._Yaw_Gyro_I = 10;
    pidinf._Yaw_Gyro_D = 0;
    pidinf._Yaw_Gyro_Imax = 150;

    pidinf._AltHold_Dist_P = 60;
    pidinf._AltHold_Speed_P = 350;
    pidinf._AltHold_Acc_P = 25;
    pidinf._AltHold_Acc_I = 15;
    pidinf._AltHold_Acc_Imax = 200;

    pidinf._Loiter_Dist_P = 70;
    pidinf._Loiter_Speed_P = 120;
    pidinf._Loiter_Acc_P = 40;
    pidinf._Loiter_Acc_I = 20;
    pidinf._Loiter_Brake_Gyro = 20;

    for (i = 0; i < RC_CALIB_CH_NUM; i++)
    {
        pidinf._rc_cal_offset[i] = 1500;      // 初始中立值 1500
        pidinf._rc_cal_factor_up[i] = 1000;   // 初始补偿因数1000   /1000 = 1
        pidinf._rc_cal_factor_down[i] = 1000; // 初始补偿因数1000   /1000 = 1
    }
}

// @brief flash 读取 pid 参数信息
void flash_read_pidinf(void)
{
    unsigned char i = 0;
    // unsigned int testi[2] = {0};
    // testi[0] = 0x12345678;
    // testi[1] = 0x12345678;

    // flash_write_bytes(pidinf_addr, (uint8_t*)&testi, sizeof(testi));
    flash_read_bytes(pidinf_addr, (uint8_t *)&pidinf, sizeof(pidinf));
    if (pidinf._pid_flag != (int16_t)0xA12A)
    {                          // 配置表是否有效？
        load_default_pidinf(); // 装载默认的配置，并写入Flash
        flash_write_bytes(pidinf_addr, (uint8_t *)&pidinf, sizeof(pidinf));
    }

    pid_v_roll.angle_p = pidinf._Roll_Angle_P / 100.0f; // 1.2
    pid_v_roll.gyro_p = pidinf._Roll_Gyro_P / 100.0f; // 4.5
    pid_v_roll.gyro_i = 0.05f;
    pid_v_roll.gyro_d = pidinf._Roll_Gyro_D / 100.0f; // 0.06
    pid_v_roll.gyro_imax = 150.0f;

    pid_v_pitch.angle_p = pidinf._Pitch_Angle_P / 100.0f;
    pid_v_pitch.gyro_p = pidinf._Pitch_Gyro_P / 100.0f;
    pid_v_pitch.gyro_i = 0.05f;
    pid_v_pitch.gyro_d = pidinf._Pitch_Gyro_D / 100.0f;
    pid_v_pitch.gyro_imax = 150.0f;

    pid_v_yaw.angle_p = pidinf._Yaw_Angle_P / 100.0f;
    pid_v_yaw.gyro_p = pidinf._Yaw_Gyro_P / 100.0f;
    pid_v_yaw.gyro_i = 0.10f;
    pid_v_yaw.gyro_d = pidinf._Yaw_Gyro_D / 100.0f;
    pid_v_yaw.gyro_imax = 150.0f;

    pid_v_alt.dist_p = pidinf._AltHold_Dist_P / 100.0f;
    pid_v_alt.speed_p = pidinf._AltHold_Speed_P / 100.0f;
    pid_v_alt.acc_p = pidinf._AltHold_Acc_P / 100.0f;
    pid_v_alt.acc_i = pidinf._AltHold_Acc_I / 100.0f;
    pid_v_alt.brake_gyro = pidinf._AltHold_Acc_Imax / 10.0f;

    pid_v_pos.dist_p = pidinf._Loiter_Dist_P / 100.0f;
    pid_v_pos.speed_p = pidinf._Loiter_Speed_P / 100.0f;
    pid_v_pos.acc_p = pidinf._Loiter_Acc_P / 100.0f;
    pid_v_pos.acc_i = pidinf._Loiter_Acc_I / 100.0f;
    pidinf._Loiter_Brake_Gyro =
        constrain_int16(pidinf._Loiter_Brake_Gyro, 10, 50);
    pid_v_pos.brake_gyro = pidinf._Loiter_Brake_Gyro / 10.0f;

    // 如果中立位值超过范围，视为错误，恢复默认值
    if (((pidinf._rc_cal_offset[RC_ROLL] > 1800) ||
         (pidinf._rc_cal_offset[RC_ROLL] < 1200)) ||
        ((pidinf._rc_cal_offset[RC_PITCH] > 1800) ||
         (pidinf._rc_cal_offset[RC_PITCH] < 1200)) ||
        ((pidinf._rc_cal_offset[RC_THR] > 1800) ||
         (pidinf._rc_cal_offset[RC_THR] < 1200)) ||
        ((pidinf._rc_cal_offset[RC_YAW] > 1800) ||
         (pidinf._rc_cal_offset[RC_YAW] < 1200)))
    {
        for (i = 0; i < RC_CALIB_CH_NUM; i++)
        {
            rc_cal_offset[i] = 1500;
            rc_cal_factor_up[i] = 1000;
            rc_cal_factor_down[i] = 1000;
        }
    }
    else
    {
        for (i = 0; i < RC_CALIB_CH_NUM; i++)
        {
            rc_cal_offset[i] = pidinf._rc_cal_offset[i];
            rc_cal_factor_up[i] = pidinf._rc_cal_factor_up[i];
            rc_cal_factor_down[i] = pidinf._rc_cal_factor_down[i];
        }
    }
}

// @brief flash 写入 pid 信息发
void flash_write_pidinf(void)
{
    unsigned char i = 0;

    pidinf._pid_flag = (int16_t)0xA12A; // 数据有效标志

    //================数据赋值部分=============
    pid_v_roll.angle_p = pidinf._Roll_Angle_P / 100.0f;
    pid_v_roll.gyro_p = pidinf._Roll_Gyro_P / 100.0f;
    pid_v_roll.gyro_i = 0.05f;
    pid_v_roll.gyro_d = pidinf._Roll_Gyro_D / 100.0f;
    pid_v_roll.gyro_imax = 150.0f;

    pid_v_pitch.angle_p = pidinf._Pitch_Angle_P / 100.0f;
    pid_v_pitch.gyro_p = pidinf._Pitch_Gyro_P / 100.0f;
    pid_v_pitch.gyro_i = 0.05f;
    pid_v_pitch.gyro_d = pidinf._Pitch_Gyro_D / 100.0f;
    pid_v_pitch.gyro_imax = 150.0f;

    pid_v_yaw.angle_p = pidinf._Yaw_Angle_P / 100.0f;
    pid_v_yaw.gyro_p = pidinf._Yaw_Gyro_P / 100.0f;
    pid_v_yaw.gyro_i = 0.10f;
    pid_v_yaw.gyro_d = pidinf._Yaw_Gyro_D / 100.0f;
    pid_v_yaw.gyro_imax = 150.0f;

    pid_v_alt.dist_p = pidinf._AltHold_Dist_P / 100.0f;
    pid_v_alt.speed_p = pidinf._AltHold_Speed_P / 100.0f;
    pid_v_alt.acc_p = pidinf._AltHold_Acc_P / 100.0f;
    pid_v_alt.acc_i = pidinf._AltHold_Acc_I / 100.0f;
    pid_v_alt.brake_gyro = pidinf._AltHold_Acc_Imax / 10.0f;

    pid_v_pos.dist_p = pidinf._Loiter_Dist_P / 100.0f;
    pid_v_pos.speed_p = pidinf._Loiter_Speed_P / 100.0f;
    pid_v_pos.acc_p = pidinf._Loiter_Acc_P / 100.0f;
    pid_v_pos.acc_i = pidinf._Loiter_Acc_I / 100.0f;
    pidinf._Loiter_Brake_Gyro =
        constrain_int16(pidinf._Loiter_Brake_Gyro, 10, 50);
    pid_v_pos.brake_gyro = pidinf._Loiter_Brake_Gyro / 10.0f;
    //====================================================

    for (i = 0; i < RC_CALIB_CH_NUM; i++)
    {
        pidinf._rc_cal_offset[i] = rc_cal_offset[i];
        pidinf._rc_cal_factor_up[i] = rc_cal_factor_up[i];
        pidinf._rc_cal_factor_down[i] = rc_cal_factor_down[i];
    }

    if (thr_lock_status == LOCKED)
    {
        flash_write_bytes(pidinf_addr, (uint8_t *)&pidinf, sizeof(pidinf));
    }
    else
    {
        return;
    }
}

// @brief 装载默认的版本信息
void load_default_verinf(void)
{
    verinf._ver_flag = (int16_t)0xA11A;

    memcpy(verinf._ap_name, NM_VER, sizeof(verinf._ap_name));
    verinf._serial_id = 001;

    verinf._sysid = 0;
}

// @brief flash 读取版本信息
void flash_read_verinf(void)
{
    flash_read_bytes(verinf_addr, (uint8_t *)&verinf, sizeof(verinf));
    if (verinf._ver_flag != (int16_t)0xA11A)
    {                          // 配置表是否有效？
        load_default_verinf(); // 装载默认的配置，并写入Flash
        flash_write_bytes(verinf_addr, (uint8_t *)&verinf, sizeof(verinf));
    }

    memcpy(ver_par.ap_name, verinf._ap_name, sizeof(verinf._ap_name));
    ver_par.serial_id = verinf._serial_id;

    ver_par.hardware_id = HW_VER;
    ver_par.firmware_id = FW_VER;
}

// @brief flash 写入版本信息
void flash_write_verinf(void)
{
    verinf._ver_flag = (int16_t)0xA11A; // 数据有效标志

    //==================数据赋值部分===============
    memcpy(ver_par.ap_name, verinf._ap_name, sizeof(verinf._ap_name));
    ver_par.serial_id = verinf._serial_id;

    ver_par.hardware_id = HW_VER;
    ver_par.firmware_id = FW_VER;
    //==============================================

    flash_write_bytes(verinf_addr, (uint8_t *)&verinf, sizeof(verinf));
}

// @brief 装载默认的机型配置信息
void load_default_confinf(void)
{
    confinf._conf_flag = (int16_t)0xA14A;

    confinf._jixing = 42;         // 默认机型为4旋翼X型
    confinf._lowvolt_flag = 2;    // 默认一级电压保护返航
    confinf._lowvolt_value = 111; // 11.1V
    confinf._lowvolt_flag2 = 3;
    confinf._lowvolt_value2 = 0;
    confinf._lostlink_time = 0;        // 默认不开启，失去联系 返航 单位s
    confinf._circle_radius = 12;       // 默认盘旋半径12m
    confinf._takeoff_alt = 5;          // 默认起飞高度5m
    confinf._rth_alt = 10;             // 默认返航高度10m
    confinf._mag_offset = 0;           // 默认磁偏角补偿0
    confinf._enable_infrared = 10;     // 默认雷达灵敏度10
    confinf._idle_speed_level = 2;     // 默认怠速2档 1150
    confinf._port4_af_function = 0;    // 默认串口 4 复用功能透传
    confinf._rc_fail_loiter_time = 10; // 默认遥控器失控后的悬停时间 10s
}

// @brief flash 读取机型配置信息
void flash_read_confinf(void)
{
    flash_read_bytes(confinf_addr, (uint8_t *)&confinf, sizeof(confinf));
    if (confinf._conf_flag != (int16_t)0xA14A)
    {                           // 配置表是否有效？
        load_default_confinf(); // 装载默认的配置，并写入Flash
        flash_write_bytes(confinf_addr, (uint8_t *)&confinf, sizeof(confinf));
    }

    conf_par.jixing = confinf._jixing;
    conf_par.lowvolt_flag = confinf._lowvolt_flag;
    conf_par.lowvolt_value = confinf._lowvolt_value;
    conf_par.lostlink_time = confinf._lostlink_time;
    conf_par.circle_radius = confinf._circle_radius * 100;
    // 环绕半径不小于500cm
    if (conf_par.circle_radius < 500)
    {
        conf_par.circle_radius = 500;
    }
    conf_par.rth_alt_cm = confinf._rth_alt * 100;
    conf_par.takeoff_alt_cm = confinf._takeoff_alt * 100;
    conf_par.mag_offset = confinf._mag_offset;
    conf_par.enable_infrared = confinf._enable_infrared;

    // 根据怠速级别设置怠速数值
    if (confinf._idle_speed_level >= 1 && confinf._idle_speed_level <= 5)
        conf_par.idle_speed = 1050 + (confinf._idle_speed_level - 1) * 50;
    else
    {
        conf_par.idle_speed = 1100;
        confinf._idle_speed_level = 2;
    }
    conf_par.port4_af_function = confinf._port4_af_function;
    // conf_par.dpgs_direction = confinf._dgps_direction;
    conf_par.lowvolt_flag2 = confinf._lowvolt_flag2;
    conf_par.lowvolt_value2 = confinf._lowvolt_value2;
    conf_par.rc_fail_loiter_time = confinf._rc_fail_loiter_time;
}

// @brief flash 写入机型配置信息
void flash_write_confinf(void)
{
    confinf._conf_flag = (int16_t)0xA14A; // 数据有效标志

    //===============数据赋值部分=======================
    conf_par.jixing = confinf._jixing;
    conf_par.lowvolt_flag = confinf._lowvolt_flag;
    conf_par.lowvolt_value = confinf._lowvolt_value;
    conf_par.lostlink_time = confinf._lostlink_time;
    conf_par.circle_radius = confinf._circle_radius * 100;
    // 环绕半径不小于500cm
    if (conf_par.circle_radius < 500)
    {
        conf_par.circle_radius = 500;
    }
    conf_par.rth_alt_cm = confinf._rth_alt * 100;
    conf_par.takeoff_alt_cm = confinf._takeoff_alt * 100;
    conf_par.mag_offset = confinf._mag_offset;
    conf_par.enable_infrared = confinf._enable_infrared;
    // 根据怠速级别设置怠速数值
    if (confinf._idle_speed_level >= 1 && confinf._idle_speed_level <= 5)
        conf_par.idle_speed = 1050 + (confinf._idle_speed_level - 1) * 50;
    else
    {
        conf_par.idle_speed = 1100;
        confinf._idle_speed_level = 2;
    }

    // conf_par.center_compensation_x = confinf._center_compensation_x;
    // conf_par.center_compensation_y = confinf._center_compensation_y;
    // conf_par.center_compensation_z = confinf._center_compensation_z;
    conf_par.port4_af_function = confinf._port4_af_function;
    // conf_par.dpgs_direction = confinf._dgps_direction;
    //====================================================
    conf_par.lowvolt_flag2 = confinf._lowvolt_flag2;
    conf_par.lowvolt_value2 = confinf._lowvolt_value2;
    conf_par.rc_fail_loiter_time = confinf._rc_fail_loiter_time;

    flash_write_bytes(confinf_addr, (uint8_t *)&confinf, sizeof(confinf));
}


// @brief flash 写入自定义禁飞区信息
// @param custom_no_fly_zone_total_num 自定义禁飞区总条数
// @param current_no_fly_zone_num 要写入的禁飞区是第几条
// @param current_no_fly_zone_vertex_num 要写入的禁飞区的顶点个数
// @param *pdata 要写入的禁飞区顶点的坐标信息指针
void flash_write_custom_no_fly_zone(uint8_t custom_no_fly_zone_total_num,
                                    uint8_t current_no_fly_zone_num,
                                    uint8_t current_no_fly_zone_vertex_num,
                                    Coords3dint *pdata)
{
    static uint8_t write_buffer[SIZE_OF_ONE_NO_FLY_ZONE] = {0};

    uint16_t no_fly_zone_useable_flag = (uint16_t)0xACCA;
    uint16_t index = 0;
    uint32_t write_addr =
        NO_FLY_ZONE_ADDR + (current_no_fly_zone_num)*SIZE_OF_ONE_NO_FLY_ZONE;
    short2buf(&write_buffer[index], (short *)&no_fly_zone_useable_flag);
    index += sizeof(uint16_t);
    write_buffer[index++] = custom_no_fly_zone_total_num;
    write_buffer[index++] = current_no_fly_zone_vertex_num;
    if (current_no_fly_zone_vertex_num <= 8)
    {
        memcpy(&write_buffer[index], pdata,
               current_no_fly_zone_vertex_num * sizeof(Coords3dint));

        flash_write_bytes(write_addr, write_buffer, sizeof(write_buffer));
    }
}

// @brief flash 读取禁飞区信息
// @param read_no_fly_zone_num 要读取的禁飞区的序号 0 开始数
// @param *custom_no_fly_zone_total_num 读出来的禁飞区总个数
// @param *current_no_fly_zone_vertex_num 读出来的当前禁飞区顶点个数
// @param *pdata 读出来的顶点数据存放地址指针
static bool flash_read_custom_no_fly_zone(
    uint8_t read_no_fly_zone_num, uint8_t *custom_no_fly_zone_total_num,
    uint8_t *current_no_fly_zone_vertex_num, Coords3dint *pdata)
{
    uint32_t read_addr =
        NO_FLY_ZONE_ADDR + (read_no_fly_zone_num)*SIZE_OF_ONE_NO_FLY_ZONE;
    uint16_t useableFlag;
    // 预备存储读取出来的总条数，顶点数
    uint8_t totalNum, currentVertexNum;
    flash_read_bytes(read_addr, (uint8_t *)&useableFlag, sizeof(useableFlag));
    flash_read_bytes(read_addr + sizeof(useableFlag), &totalNum,
                     sizeof(totalNum));
    flash_read_bytes(read_addr + sizeof(useableFlag) + sizeof(totalNum),
                     &currentVertexNum, sizeof(currentVertexNum));
    // 如果存储有效
    if (useableFlag == 0xACCA && totalNum <= 5 && currentVertexNum <= 8)
    {
        // 如果是在读第一个位置存储的禁飞区，则更新一下自定义禁飞区的总条数
        if (read_no_fly_zone_num == 0)
        {
            *custom_no_fly_zone_total_num = totalNum;
        }
        // 更新一下自定义禁飞区的定点个数
        *current_no_fly_zone_vertex_num = currentVertexNum;
        // 读出顶点的坐标数据
        flash_read_bytes(read_addr + sizeof(useableFlag) + sizeof(totalNum) +
                             sizeof(currentVertexNum),
                         (uint8_t *)pdata,
                         currentVertexNum * sizeof(Coords3dint));
        return true;
    }
    else
    {
        useableFlag = 0x0000;
        flash_write_bytes(read_addr, (uint8_t *)&useableFlag,
                          sizeof(useableFlag));
        return false;
    }
}

/**
 * @brief 装载默认的 par 参数
 */
void load_default_parinf(void)
{
    parinf._par_flag = (int16_t)0xAAEE; // 数据有效标志
    parinf._par_maxangle = 25;          // 最大角度 30 deg
    parinf._par_maxhspeed = 80;         // GPS 推杆最大水平速度 dm/s
    parinf._par_maxupspeed = 40;        // GPS 推杆最大上升速度 dm/s
    parinf._par_maxdownspeed = 30;      // GPS 推杆最大下降速度 dm/s
    parinf._par_hover_throttle = 1400;  // 默认悬停油门
    parinf._par_alt_fs_rth = 2000;      // 高度限制，1级保护。返航
    parinf._par_alt_fs_land = 2500;     // 高度限制，2级保护。迫降
    parinf._par_maxyawrate = 30;        // 最大自动转航向角速度
    parinf._par_volt_offset[0] = 0;     // 底板电压采集有压降, 0.3v
    parinf._par_volt_offset[1] = 0;
    parinf._par_volt_offset[2] = 0;
    parinf._par_volt_offset_b[0] = 0; // 底板电压采集有压降, 0.3v
    parinf._par_volt_offset_b[1] = 0;
    parinf._par_volt_offset_b[2] = 0;
    parinf._fllow_dist = 10;
    // 自动模式最大爬升速率 dm/s
    parinf._par_max_climb_rate_automode = 20;
    // 自动模式最大下降目标速率 dm/s
    parinf._par_max_approach_rate_automode = 15;
    // 自动模式降落最小目标垂直速率 dm/s
    parinf._par_min_landing_rate_automode = 4;
    // 自动模式巡航最大水平速率 dm/s
    parinf._par_max_rth_horizontal_speed = 80;
    // 电压校准系数
    parinf._par_volt_kp[0] = 1000;
    parinf._par_volt_kp[1] = 1000;
    parinf._par_volt_kp[2] = 1000;
    // 最大水平加加速度
    parinf._max_hor_jet = 15;
    parinf._bms_low_capacity_percentage = 30;
    parinf._motor_failsafe_action = 0;
}

/**
 * @brief 从 flash 读取 par 参数
 */
void flash_read_parinf(void)
{
    flash_read_bytes(parinf_addr, (uint8_t *)&parinf, sizeof(parinf));
    if (parinf._par_flag != (int16_t)0xAAEE)
    {                          // 配置表是否有效？
        load_default_parinf(); // 装载默认的配置，并写入Flash
        flash_write_bytes(parinf_addr, (uint8_t *)&parinf, sizeof(parinf));
    }

    //=============数据赋值部分================
    if (params_get_value(ParamNum_APMaxTilteAngleDeg) < 15)
    {
        params_set_value(ParamNum_APMaxTilteAngleDeg, 15);
    }
    else if (params_get_value(ParamNum_APMaxTilteAngleDeg) > 45)
    {
        params_set_value(ParamNum_APMaxTilteAngleDeg, 45);
    }

    if (parinf._par_maxhspeed < 30)
    {
        parinf._par_maxhspeed = 30;
    }
    else if (parinf._par_maxhspeed > 150)
    {
        parinf._par_maxhspeed = 150;
    }

    // 对跟随距离做限制
    if (parinf._fllow_dist < 3)
    {
        parinf._fllow_dist = 3;
    }
    else if (parinf._fllow_dist > 100)
    {
        parinf._fllow_dist = 100;
    }

    // 最大自动目标爬升速率在 1~5 m/s
    if (parinf._par_max_climb_rate_automode < 10)
    {
        parinf._par_max_climb_rate_automode = 10;
    }
    else if (parinf._par_max_climb_rate_automode > 50)
    {
        parinf._par_max_climb_rate_automode = 50;
    }

    // 最大自动目标下降速率在 1~5 m/s
    if (parinf._par_max_approach_rate_automode < 10)
    {
        parinf._par_max_approach_rate_automode = 10;
    }
    else if (parinf._par_max_approach_rate_automode > 50)
    {
        parinf._par_max_approach_rate_automode = 50;
    }

    // 最小目标降落速度在 0.2 ~ 0.5 m/s
    if (parinf._par_min_landing_rate_automode < 2)
    {
        parinf._par_min_landing_rate_automode = 2;
    }
    else if (parinf._par_min_landing_rate_automode > 5)
    {
        parinf._par_min_landing_rate_automode = 5;
    }

    // 最大目标航线水平速度在 10~20 m/s
    if (parinf._par_max_rth_horizontal_speed < 20 ||
        parinf._par_max_rth_horizontal_speed > 300)
    {
        parinf._par_max_rth_horizontal_speed = 80;
    }

    if (parinf._max_hor_jet < 4 || parinf._max_hor_jet > 50)
    {
        parinf._max_hor_jet = 50;
    }

    //=========================================
}

/**************************实现函数********************************************
 *函数原型:		void flash_write_jfqinf(void)
 *功　　能:	    将当前的拍照参数设置保存到Flash，永久保存
 *******************************************************************************/
void flash_write_parinf(void)
{
    parinf._par_flag = (int16_t)0xAAEE; // 数据有效标志

    //=============数据赋值部分================

    if (params_get_value(ParamNum_APMaxTilteAngleDeg) < 15)
    {
        params_set_value(ParamNum_APMaxTilteAngleDeg, 15);
    }
    else if (params_get_value(ParamNum_APMaxTilteAngleDeg) > 45)
    {
        params_set_value(ParamNum_APMaxTilteAngleDeg, 45);
    }

    if (parinf._par_maxhspeed < 30)
    {
        parinf._par_maxhspeed = 30;
    }
    else if (parinf._par_maxhspeed > 150)
    {
        parinf._par_maxhspeed = 150;
    }

    // 对跟随距离做限制
    if (parinf._fllow_dist < 3)
    {
        parinf._fllow_dist = 3;
    }
    else if (parinf._fllow_dist > 100)
    {
        parinf._fllow_dist = 100;
    }

    // 最大自动目标爬升速率在 1~5 m/s
    if (parinf._par_max_climb_rate_automode < 10)
    {
        parinf._par_max_climb_rate_automode = 10;
    }
    else if (parinf._par_max_climb_rate_automode > 50)
    {
        parinf._par_max_climb_rate_automode = 50;
    }

    // 最大自动目标下降速率在 1~5 m/s
    if (parinf._par_max_approach_rate_automode < 10)
    {
        parinf._par_max_approach_rate_automode = 10;
    }
    else if (parinf._par_max_approach_rate_automode > 50)
    {
        parinf._par_max_approach_rate_automode = 50;
    }

    // 最小目标降落速度在 0.2 ~ 0.5 m/s
    if (parinf._par_min_landing_rate_automode < 2)
    {
        parinf._par_min_landing_rate_automode = 2;
    }
    else if (parinf._par_min_landing_rate_automode > 5)
    {
        parinf._par_min_landing_rate_automode = 5;
    }

    // 最大目标航线水平速度在 10~20 m/s
    if (parinf._par_max_rth_horizontal_speed < 20 ||
        parinf._par_max_rth_horizontal_speed > 300)
    {
        parinf._par_max_rth_horizontal_speed = 80;
    }

    if (parinf._max_hor_jet < 4 || parinf._max_hor_jet > 50)
    {
        parinf._max_hor_jet = 50;
    }

    flash_write_bytes(parinf_addr, (uint8_t *)&parinf, sizeof(parinf));
}


/**************************实现函数********************************************
*函数原型:		void initial_parameters(void)
*功　　能:	    初始化四轴的数据参数。
读取PID参数，以便在四轴控制中进行运算
*******************************************************************************/
void initial_parameters(void)
{
    // 读取PID参数
    flash_read_pidinf();

    // 读取飞控版本信息
    flash_read_verinf();

    // 读取机型配置信息
    flash_read_confinf();


    // 读取备用参数信息
    flash_read_parinf();
}

/* 需要记录参数的标志位 */
bool write_pid_information = false, write_conf_information = false,
     write_cam_information = false, write_ver_information = false,
      write_breakpoint_information = false,
     write_par_information = false, write_pos_information = false,
     write_postotal_information = false, write_iap_flag = false,
     write_rcfactor_flag = false, write_custom_noflyzone = false;

/**
 * @brief 参数写入 flash
 *
 */
void write_par_to_flash(void)
{

    if (thr_lock_status != LOCKED)
    {
        if (write_pid_information == true) // 写PID参数信息
        {
            // FLASH忙则先返回。
            flash_write_pidinf();
        }
        return;
    }

    if (write_pid_information == true) // 写PID参数信息
    {
        // FLASH忙则先返回。
        if (flash_isbusy())
            return;
        flash_write_pidinf();
        write_pid_information = false;
    }
    else if (write_ver_information == true) // 写版本信息
    {
        // FLASH忙则先返回。
        if (flash_isbusy())
            return;
        flash_write_verinf();
        write_ver_information = false;
    }
    else if (write_conf_information == true) // 写机型配置信息
    {
        // FLASH忙则先返回。
        if (flash_isbusy())
            return;
        flash_write_confinf();
        write_conf_information = false;
    }
    else if (write_par_information == true) // 写备用参数信息
    {
        // FLASH忙则先返回。
        if (flash_isbusy())
            return;
        flash_write_parinf();
        write_par_information = false;
    }
    else if (write_rcfactor_flag == true) // 写遥控器校准系数信息
    {
        // 计算遥控器补偿的系数
        calc_rcfactor_calibration();
        // 写入flash
        flash_write_pidinf();
        delay_ms(200);

        write_rcfactor_flag = false;

        pilot_mode = PILOT_NORMAL;
    }    
    else if (write_iap_flag == true) // 写IAP升级信息
    {
        switch (ver_par.hardware_id)
        {
        case HW_V8M_YY:
            V8M_record_iap_flag();
            break;

        default:
            return;
        }

        write_iap_flag = false;
        // 升级前失能电平芯片输出，PWM脉宽给0
        gd25q16_cs_high();
        for (uint8_t i = 1; i <= params_get_value(ParamNum_APType) / 10; i++)
        {
            set_motor_pwm(i, 0);
        }
        delay_ms(200);

        // 软件复位
        system_reset();
      
    
    }
}

//------------------End of File----------------------------