1、修改了工程文件，包含了所有所需文件

2、SD卡能识别初始化了，但是挂载不了
3、flash的分配要重新弄，hpm6750的xpi nor flash定义了存放用户代码的地址是从0x80003000开始
4、能编译的都编译了，串口驱动也弄了，后面就是把gs和数传的补上、接着调sd卡
5、断点调试很鸡肋，好多地方打不了断点，不知道原因。特别是sd等库，能看源文件，打不进去断点？？？？

controller_yy_app/controlware/auto_pilot.c

@@ -0,0 +1,523 @@
+#include "auto_pilot.h"
+#include "common.h"
+#include "control_rate.h"
+#include "control_throttle.h"
+#include "data_save.h"
+#include "flight_mode.h"
+#include "my_math.h"
+#include "params.h"
+#include "pilot_navigation.h"
+#include "soft_flash.h"
+#include "soft_gs.h"
+#include "soft_imu.h"
+#include "soft_motor_output.h"
+#include "soft_port_uart4.h"
+#include "soft_rc_input.h"
+#include "soft_sdcard.h"
+#include "soft_time.h"
+#include "soft_timer.h"
+#include "soft_voltage.h"
+#include "soft_warn.h"
+#include "ver_config.h"
+#include "helpler_funtions.h"
+#include <math.h>
+#include <stdlib.h>
+
+char pilot_mode = PILOT_NORMAL;
+
+char warn_reason = WARN_NO;
+
+uint8_t home_pos_isrecord = HOME_POS_NOT_SET;
+uint8_t takeoff_pos_isrecord = 0;
+
+// 本次上电飞行时间
+static float have_fly_time = 0.0f;
+// 本架次飞行时间
+static float have_fly_time_this_sort = 0.0f;
+
+// @brief 5hz计算解锁飞行的时间
+static void calculate_have_fly_time_5hz(void)
+{
+    static unsigned int record_fly_time = 0;
+
+    if (thr_lock_status == LOCKED)
+    {
+        record_fly_time = micros();
+        have_fly_time_this_sort = 0.0f;
+    }
+    else if (thr_lock_status == UNLOCKED)
+    {
+        float dt = (float)(micros() - record_fly_time) / 1000000.0f;
+        if (dt < 0.0f)
+        {
+            dt = 0.2f;
+        }
+        have_fly_time += dt;
+        have_fly_time_this_sort += dt;
+        record_fly_time = micros();
+    }
+}
+
+// @brief 获取总飞行时间 s
+float Get_HaveFlyTime(void) { return have_fly_time; }
+
+// @brief 获取本架次飞行时间 s
+float Get_HaveFlyTimeThisSort(void) { return have_fly_time_this_sort; }
+
+static float have_fly_miles_this_sort = 0;
+static float have_fly_miles = 0;
+
+// 已飞航线里程
+static float have_fly_wp_miles;
+
+// @brief 判断是否使用遥控器进入磁校准
+void judge_mag_calibrateion(void)
+{
+    static short preinput_ch5_mag = 1000;
+    static uint8_t magcal_change_counts = 0;
+    static uint32_t magcal_time = 0;
+
+    if ((rc_in[RC_CH5] >= MAX_S && rc_in[RC_CH5] <= MAX_E) &&
+        (preinput_ch5_mag >= MIN_S && preinput_ch5_mag <= MIN_E))
+    {
+        magcal_change_counts++;
+        preinput_ch5_mag = rc_in[RC_CH5];
+        magcal_time = micros();
+    }
+    else if ((rc_in[RC_CH5] >= MIN_S && rc_in[RC_CH5] <= MIN_E) &&
+             (preinput_ch5_mag >= MAX_S && preinput_ch5_mag <= MAX_E))
+    {
+        magcal_change_counts++;
+        preinput_ch5_mag = rc_in[RC_CH5];
+        magcal_time = micros();
+    }
+
+    // 5通道连续切换8次，且两次之间间隔小于1.5s，则识别为需要磁校准
+    // 实际能识别7次就可以了。
+    if (magcal_change_counts >= 8)
+    {
+        // 上锁状态并且未进入磁校准下才能执行，防止连续点击多次发送
+        if (thr_lock_status == LOCKED && mag_cal_status == MAG_CAL_NO &&
+            pilot_mode == PILOT_NORMAL)
+        {
+            ci_jiao_zhun = true;
+            magcal_change_counts = 0;
+        }
+    }
+
+    // 如果1.5s之内没有切换，则清零计数
+    if (micros() - magcal_time >= 1500000)
+    {
+        magcal_change_counts = 0;
+        magcal_time = micros();
+    }
+}
+
+// @brief 判断是否使用遥控器进入遥控器校准
+void judge_rc_calibration(void)
+{
+    static short preinput_CH7_rc = 1000;
+    static unsigned char rccal_change_counts = 0;
+    static unsigned int rccal_time = 0;
+
+    // 解锁时不允许校准遥控器
+    if (thr_lock_status == UNLOCKED)
+        return;
+
+    if (((raw_rc_in[RC_CH7] >= MAX_S && raw_rc_in[RC_CH7] <= MAX_E) &&
+         (preinput_CH7_rc >= MIN_S && preinput_CH7_rc <= MIN_E)) ||
+        ((raw_rc_in[RC_CH7] >= MIN_S && raw_rc_in[RC_CH7] <= MIN_E) &&
+         (preinput_CH7_rc >= MAX_S && preinput_CH7_rc <= MAX_E)))
+    {
+        rccal_change_counts++;
+        preinput_CH7_rc = raw_rc_in[RC_CH7];
+        rccal_time = micros();
+    }
+
+    // 6通道切换输出8次
+    if (rccal_change_counts >= 8)
+    {
+        rccal_change_counts = 0;
+        if (rc_cal_flag == RC_CALIB_NO)
+        {
+            // 赋值进入遥控器校准模式
+            pilot_mode = PILOT_RC_CLB;
+            rc_cal_flag = RC_CALIB_START;
+            rc_offset_capture_flag = true;
+            // 点击遥控器校准后需要延时一段时间，，不然会记录一个原先的值。填充滤波新数据额需要时间，，，
+            rc_cal_time = micros();
+        }
+        else if (rc_cal_flag == RC_CALIB_START)
+        {
+            rc_cal_flag = RC_CALIB_NO;
+            write_rcfactor_flag = true;
+        }
+    }
+
+    if (micros() - rccal_time >= 1500000)
+    {
+        rccal_change_counts = 0;
+        rccal_time = micros();
+    }
+}
+
+//===================================================================
+//
+//================获取警告信息=====================================
+//
+//=====================================================================
+void get_warn_info(void)
+{
+    int rcunlock_allow_flag = judge_pilot_rcunlock_allow();
+
+    if (imu_link_status != COMP_NORMAL)
+    {
+        set_warn_flag_bit(WARN_NO_IMU);
+    }
+    else
+    {
+        reset_warn_flag_bit(WARN_NO_IMU);
+    }
+
+    if (ins.temprature > 80)
+    {
+        set_warn_flag_bit(WARN_HIGH_TEMPERATURE);
+    }
+    else
+    {
+        reset_warn_flag_bit(WARN_HIGH_TEMPERATURE);
+    }
+
+    if (gs_get_link_status(&gcs_link) == COMP_LOST)
+    {
+        set_warn_flag_bit(WARN_LINK_LOST);
+    }
+    else
+    {
+        reset_warn_flag_bit(WARN_LINK_LOST);
+    }
+
+    if (imu_link_status != COMP_NORMAL)
+    {
+        warn_reason = WARN_NOIMU;
+    }
+    else if (dma_iserror == true)
+    {
+        warn_reason = WARN_DMA;
+    }
+    else if (ins.temprature > 80)
+    {
+        warn_reason = WARN_OVER_TEMPRATURE;
+    }
+    else if (battary_volt_islow == true)
+    {
+        warn_reason = WARN_VOLT;
+    }
+    else if (COMP_LOST == gs_get_link_status(&gcs_link))
+    {
+        warn_reason = WARN_GS;
+    }
+    else if (rcunlock_allow_flag == UNLOCK_RCERR)
+    {
+        warn_reason = WARN_RC;
+    }
+    else if (ins.insgps_ok == F_AHRS)
+    {
+        warn_reason = WARN_AHRS;
+    }
+    else if (unlock_fail == true)
+    {
+        warn_reason = WARN_ULOCK_FAIL;
+    }
+    else if (sdcard_initok() != 1)
+    {
+        warn_reason = WARN_SD_ERROR;
+    }
+    else
+    {
+        warn_reason = WARN_NO;
+    }
+}
+
+/**************************实现函数********************************************
+ *函数原型:		void rate_control(void)
+ *功　　能:	  底层的陀螺角速度控制 调用频率400hz，一次运行时间11us
+ *******************************************************************************/
+
+void rate_control(void)
+{
+    static unsigned int pid_time = 0;
+
+    float pid_dt = 0.0f;
+
+    pid_dt = time_interval(micros(), &pid_time) / 1000000.0f;
+    pid_time = micros();
+
+    pid_roll = rate_pid_ctl_rp(&pid_m_roll, &pid_v_roll, pid_dt); // delta_t 是时间间隔，单位是秒
+    pid_pitch = -rate_pid_ctl_rp(&pid_m_pitch, &pid_v_pitch, pid_dt);
+    pid_yaw = rate_pid_ctl_yaw(&pid_m_yaw, &pid_v_yaw, pid_dt);
+
+    rate_pid_acctothrottle_alt(pid_dt);
+    pid_thr = rate_pid_ctl_thr();
+}
+
+/*
+1hz 低频率计算一些数据
+*/
+void loop_1_hz(void)
+{
+    if (hz_1_flag == true)
+    {
+        hz_1_flag = false;
+
+        // 检测 imu 连接状态
+        get_ins_status();
+
+        // 获取警告信息
+        get_warn_info();
+    }
+}
+
+void loop_2_hz(void)
+{
+    if (hz_2_flag == true)
+    {
+        hz_2_flag = false;
+    }
+}
+
+/*
+正常控制模式的5hz循环
+*/
+void normal_loop_5_hz(void)
+{
+    if (hz_5_flag == true)
+    {
+        hz_5_flag = false;
+
+        // 计算已飞时间
+        calculate_have_fly_time_5hz();
+
+        // get_fly_data_type_data_5hz();
+
+        // 更新存储数据的buff
+        // 只有在解锁的情况下才来记录数据，不解锁就记录会有问题：
+        //  1，资源浪费.2，数据量怎多时同时200ms内FLASH无法写完会造成无法结束写入。
+#ifdef REC_DATA_POS
+        // if (thr_lock_status == UNLOCKED)
+#endif
+    }
+}
+
+/*
+正常控制模式的20hz循环
+*/
+void normal_loop_10_hz(void)
+{
+    if (hz_10_flag == true)
+    {
+        hz_10_flag = false;
+
+        // 检测SBUS接收机连接状态。
+        check_sbus_link_status(0.1f);
+
+        // 判断是否进入磁校准程序
+        judge_mag_calibrateion();
+
+        // 判断是否进入遥控器校准程序
+        judge_rc_calibration();
+
+        // 解锁识别
+        pilot_unlock_decide();
+        // 起飞识别
+        pilot_takeoff_decide();
+        // 上锁识别
+        pilot_lock_decide();
+
+        get_fly_data_type_data_5hz();
+    }
+}
+
+/*
+正常控制模式的50hz循环
+*/
+void normal_loop_50_hz(void)
+{
+    if (hz_50_flag == true)
+    {
+        hz_50_flag = false;
+
+        const float dt = 1 / 50.0f;
+
+        // 更新电压采集
+        Voltage_Update();
+
+        // 获取遥控器的值及当前状态
+        get_rc_value();
+
+        // 飞行模式判断
+        flight_mode_switch();
+
+
+        // 在锁定情况下如果是要检测电机，在输出电机值
+        locked_motor_output();
+
+        // 2 biaozhi 1ge chuan dimianzhan   1ge chuan yaokongqi   flight
+        // mode flag == 条件 （1、地面站模式 flag1 2、遥控器模式 flag2）
+    }
+}
+
+/*
+正常控制模式的200hz循环
+*/
+unsigned int fast_loop_time = 0;
+const float fast_loop_dt = 1.f / 200.f;
+
+void normal_loop_200_hz(void)
+{
+    // 200hZ调用，设置飞机的工作模式
+    if (hz_200_flag == true)
+    {
+        hz_200_flag = false;
+
+        // 更新并执行飞行模式，模式识别在50hz循环中
+        if (thr_lock_status == UNLOCKED || flight_mode == GCS_VEHICLE_LAUNCH)
+        {
+            update_flight_mode();
+        }
+    }
+}
+
+/**
+ * 标准模式 400hz
+ */
+void loop_400_hz(void)
+{
+    if (hz_400_flag)
+    {
+        hz_400_flag = false;
+
+    }
+}
+
+/*
+磁校准模式 50hz的loop
+*/
+void compasscal_loop_50_hz(void)
+{
+    if (hz_50_flag == true)
+    {
+        hz_50_flag = false;
+
+        if (mag_cal_status == MAG_CAL_NO)
+        {
+            // 经常出现无法进入磁校准的状况，所以，进不去后，可以重传
+            ci_jiao_zhun = true;
+        }
+        /* 如果磁校准通过则退出磁校准模式进入普通模式 */
+        if (mag_cal_status == MAG_CAL_OK)
+        {
+            /*V8M 在此退出磁校准， V7Pro 退出磁校准模式在它的电灯循环中进行 */
+            if (ver_par.hardware_id == HW_V8M_YY)
+            {
+                pilot_mode = PILOT_NORMAL;
+                mag_cal_status = MAG_CAL_NO;
+                ci_jiao_zhun = false;
+            }
+        }
+    }
+}
+
+/*
+遥控器校准模式50hz的loop
+*/
+void rccal_loop_50_hz(void)
+{
+
+    if (hz_10_flag == true)
+    {
+        hz_10_flag = false;
+
+        judge_rc_calibration();
+    }
+
+    if (hz_50_flag == true)
+    {
+        hz_50_flag = false;
+
+        // 点击遥控器校准后需要延时一段时间，，不然会记录一个原先的值。填充滤波新数据额需要时间，，
+        if ((rc_cal_flag == RC_CALIB_START) &&
+            (micros() - rc_cal_time > 100000))
+        {
+            rc_input_calibration();
+        }
+
+        // 遥控器校准时需要校准的四个通道输出原始遥控器值,便于地面站观察
+        for (uint8_t i = 0; i < RC_CALIB_CH_NUM; i++)
+        {
+            // 赋值给tmp_rc_in来在地面站显示。
+            tmp_rc_in[i] = raw_rc_in[i];
+        }
+    }
+}
+
+/*
+电调校准模式50hz的loop
+*/
+void esccal_loop_50_hz(void)
+{
+    char i = 0;
+
+    if (hz_50_flag == true)
+    {
+        hz_50_flag = false;
+
+        // 获取遥控器的值
+        get_rc_value();
+
+        // 根据机型，全部输出CH3油门量。
+        for (i = 1; i <= conf_par.jixing / 10; i++)
+        {
+            set_motor_pwm(i, rc_in[RC_CH3]);
+        }
+    }
+}
+
+/*
+飞行器的主要控制循环函数
+*/
+void pilot_main_loop(void)
+{
+    loop_1_hz();
+
+    loop_2_hz();
+
+    loop_400_hz();
+
+    // 正常控制模式
+    if (pilot_mode == PILOT_NORMAL)
+    {
+        normal_loop_5_hz();
+        normal_loop_10_hz();
+        normal_loop_50_hz();
+        normal_loop_200_hz();
+    }
+    // 磁校准模式
+    else if (pilot_mode == PILOT_MAG_CLB)
+    {
+        compasscal_loop_50_hz();
+    }
+    // 遥控器校准模式
+    else if (pilot_mode == PILOT_RC_CLB)
+    {
+        rccal_loop_50_hz();
+    }
+    // 电调校准模式
+    else if (pilot_mode == PILOT_ESC_CLB)
+    {
+        esccal_loop_50_hz();
+    }
+}
+
+
+

controller_yy_app/controlware/control_attitude.c

@@ -1,4 +1,3 @@
-#if 0
 #include "control_attitude.h"
 #include "hpm_math.h"
 #include "auto_pilot.h"
@@ -311,4 +310,3 @@ void get_smooth_target_yaw(float argTargetYaw, float dt)
 
     pid_m_yaw.angle_t = wrap_360(pid_m_yaw.angle_t);
 }
-#endif

controller_yy_app/controlware/control_inc/auto_pilot.h

@@ -0,0 +1,103 @@
+
+#ifndef __AUTO_PILOT_H
+#define __AUTO_PILOT_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+//正常模式
+#define PILOT_NORMAL 1
+//磁校准模式
+#define PILOT_MAG_CLB 2
+//遥控器校准模式
+#define PILOT_RC_CLB 3
+//电调校准模式
+#define PILOT_ESC_CLB 4
+// imu 升级模式
+#define PILOT_IMU_UPDATE 5
+
+extern char pilot_mode;
+
+/*
+===============定义警告类型==========================
+
+枚举类型默认是4字节的。
+
+
+*/
+
+// typedef enum {
+//    WARN_DEF = 0, WARN_NO, WARN_NOIMU, WARN_DMA, WARN_RC, WARN_VOLT, WARN_GS,
+//    WARN_FLOW
+//} WARNING;
+
+//无警告
+#define WARN_NO 1
+//无IMU数据
+#define WARN_NOIMU 2
+// DMA错误
+#define WARN_DMA 3
+//遥控器位置不对警告
+#define WARN_RC 4
+//电压低警告
+#define WARN_VOLT 5
+//电台丢失警告
+#define WARN_GS 6
+//解算失败警告
+#define WARN_AHRS 8
+//加速度警告
+#define WARN_ACC 9
+//速度警告
+#define WARN_VEL 10
+// gps错误
+#define WARN_IMUGPS_ERROR 11
+// gps野值
+#define WARN_IMUGPS_OUTLIERS 12
+
+#define WARN_IMUGPS_BREAK 14
+#define WARN_IMUBARO_DIFFVEL 15
+#define WARN_IMUBARO_ERROR 16
+#define WARN_IMUBARO_OUTLIERS 17
+#define WARN_IMUAG_ACCERR 18
+#define WARN_IMUAG_GYROERR 19
+#define WARN_IMUMAG_DISTURBANCE 20
+// 高温警告
+#define WARN_OVER_TEMPRATURE 21
+// IMU版本错误
+#define WARN_IMU_VERSION_ERR 22
+//磁断开
+#define WARN_IMUMAG_BREAK 23
+// 在禁飞区中
+#define WARN_IN_NO_FLY_ZONE 24
+// 动力故障
+#define WARN_SERVO_FAIL 25
+// 解锁超时失败
+#define WARN_ULOCK_FAIL 32
+// SD卡故障
+#define WARN_SD_ERROR 33
+// 雷达有障碍物
+#define WARN_OBSTACLE 50
+
+extern char warn_reason;
+
+// 家点信息是否记录标志位
+#define HOME_POS_NOT_SET 0
+#define HOME_POS_AUTO_SET 1
+#define HOME_POS_MANUL_SET 2
+extern uint8_t home_pos_isrecord;
+extern const float fast_loop_dt;
+
+
+// 400 hz 速率环路控制
+void rate_control(void);
+
+// 主要循环回路
+void pilot_main_loop(void);
+
+// 获取飞行时间
+float Get_HaveFlyTime(void);
+// 获取本架次飞行时间
+float Get_HaveFlyTimeThisSort(void);
+
+
+#endif

controller_yy_app/controlware/control_inc/common.h

@@ -0,0 +1,20 @@
+
+#ifndef __COMMON_H
+#define __COMMON_H
+
+/// 支持的最多 航点数
+#define MAX_WP_NUM 1500
+
+typedef enum
+{
+    COMP_NOEXIST = 0,
+    COMP_NORMAL = 1,
+    COMP_LOST = 2,
+    COMP_CLOSED = 3
+} comp_status;
+
+/// 通过结构体成员寻找结构体对象本身的地址
+#define GET_CONTAINER_OF(ptr, type, member)                                    \
+    ((type *)((char *)(ptr) - (unsigned long)(&((type *)0)->member)))
+
+#endif

controller_yy_app/controlware/control_inc/data_save.h

@@ -0,0 +1,199 @@
+
+#ifndef __DATA_SAVE_H
+#define __DATA_SAVE_H
+
+#include "stdbool.h"
+#include <stdint.h>
+
+//打开记录的标志位
+extern bool data_record_open;
+//结束记录的标志位
+extern bool data_record_close;
+
+#pragma pack(1)
+typedef struct
+{
+    unsigned short sorties;                  // 架次
+    char plane_type;                         // 机型
+    int fireware_num;                        // 固件号
+    int unlock_lon;                          // 解锁经度
+    int unlock_lat;                          // 解锁纬度
+    char unlock_time_year;                   // 解锁时间 年
+    char unlock_time_month;                  // 解锁时间 月
+    char unlock_time_day;                    // 解锁时间 日
+    char unlock_time_hour;                   // 解锁时间 时
+    char unlock_time_minut;                  // 解锁时间 分
+    char unlock_time_second;                 // 解锁时间 秒
+    int lock_lon;                            // 上锁经度
+    int lock_lat;                            // 上锁纬度
+    unsigned short flight_time;              // min
+    unsigned int total_flight_time;          // min
+    unsigned short flight_mileages;          // 0.1 km
+    unsigned int total_flight_mileages;      // 0.1 km
+    unsigned short unlock_voltage;           // 0.1v
+    unsigned short lock_voltage;             // 0.1v
+    unsigned char max_auto_roll;             // deg
+    unsigned char max_auto_pitch;            // deg
+    unsigned short max_auto_horizonal_speed; // dm/s
+    char max_auto_climb_speed;               // dm/s
+    signed char max_auto_down_speed;         // dm/s
+    char max_auto_gps_num;                   // 最大gps星数
+    char min_auto_gps_num;                   // 最小gps星数
+    unsigned short gps_locating_time;        // gps搜星时间
+    char mag_error;                          // 磁错误标志
+    char acc_gyro_error;                     // 加计陀螺错误标志
+    char baro_error;                         // 气压错误标志
+    char gps_error;                          // gps错误标志
+    char lidar_error;                        // 雷达错误标志
+} FLY_LOG;
+#pragma pack()
+extern FLY_LOG fly_log;
+
+#pragma pack(1)
+typedef struct
+{
+    int solved_lon;                  // 解算经度，cm
+    int solved_lat;                  // 解算纬度，cm
+    int target_lon;                  // 目标经度，cm
+    int target_lat;                  // 目标纬度，cm
+    int raw_lon;                     // 原始经度，cm
+    int raw_lat;                     // 原始纬度，cm
+    float flight_time;               // 已飞时间，s
+    short roll;                      // 横滚角 0.1deg
+    short pitch;                     // 俯仰角 0.1deg
+    short yaw;                       // 航向角 0.1deg
+    short target_roll;               // 目标横滚 0.1deg
+    short target_pitch;              // 目标俯仰 0.1deg
+    short target_yaw;                // 目标航向 0.1deg
+    short x_gyro;                    // X陀螺 0.1deg/s
+    short y_gyro;                    // Y陀螺 0.1deg/s
+    short z_gyro;                    // Z陀螺 0.1deg/s
+    short x_acc;                     // X加速度 cm/s^2
+    short y_acc;                     // Y加速度 cm/s^2
+    short z_acc;                     // Z加速度 cm/s^2
+    short x_raw_gyro1;               // deg/s
+    short y_raw_gyro1;               // deg/s
+    short z_raw_gyro1;               // deg/s
+    short x_raw_gyro2;               // deg/s
+    short y_raw_gyro2;               // deg/s
+    short z_raw_gyro2;               // deg/s
+    short x_raw_acc1;                // cm/s^2
+    short y_raw_acc1;                // cm/s^2
+    short z_raw_acc1;                // cm/s^2
+    short x_raw_acc2;                // cm/s^2
+    short y_raw_acc2;                // cm/s^2
+    short z_raw_acc2;                // cm/s^2
+    short x_raw_mag1;                // 0.1mGause
+    short y_raw_mag1;                // 0.1mGause
+    short z_raw_mag1;                // 0.1mGause
+    short x_raw_mag2;                // 0.1mGause
+    short y_raw_mag2;                // 0.1mGause
+    short z_raw_mag2;                // 0.1mGause
+    short air_speed;                 // 空速 0.1m/s
+    short solved_alt;                // 解算当前高度 0.1m
+    short target_alt;                // 目标高度 0.1m
+    short forward_lidar;             // 前雷达距离 0.1m
+    short backward_lidar;            // 后雷达距离 0.1m
+    short down_lidar_alt;            // 下雷达高度 0.1m
+    char lidar_comp_state;           // 雷达连接状态
+    short climb_rate;                // 垂直速度 cm/s
+    short target_climb_rate;         // 目标垂直速度 cm/s
+    short east_solved_speed;         // 东西速度 cm/s
+    short north_solved_speed;        // 南北速度 cm/s
+    short target_east_solved_speed;  // 目标东西速度 cm/s
+    short target_north_solved_speed; // 目标南北速度 cm/s
+    short total_wp_number;           // 总航点数
+    short target_wp_number;          // 目标航点
+    short picture_number;            // 拍照数
+    short voltage_value;             // 电压 0.1v
+    short dist_2_home;               // 离家距离 m
+    short dist_2_target;             // 目标距离 m
+    short dist_off_track;            // 偏航距离 dm
+    short gps_alt;                   // GPS高度 dm
+    short gps_yaw;                   // GPS航向 deg
+    short gps_speed;                 // GPS速度 cm/s
+    char gps_num;                    // GPS星数
+    char gps_pdop;                   // GPSPDOP
+    unsigned char pwmin1;            // 输入CH1
+    unsigned char pwmin2;
+    unsigned char pwmin3;
+    unsigned char pwmin4;
+    unsigned char pwmin5;
+    unsigned char pwmin6;
+    unsigned char pwmin7;
+    unsigned char pwmin8;
+    unsigned char pwmout1;
+    unsigned char pwmout2;
+    unsigned char pwmout3;
+    unsigned char pwmout4;
+    unsigned char pwmout5;
+    unsigned char pwmout6;
+    unsigned char pwmout7;
+    unsigned char pwmout8;
+    char temperature;                     // 温度 度
+    char rtk_flag;                        // rtk 标志位
+    char ins_flag;                        // 解算标志位
+    char lock_flag;                       // 锁标志
+    char pos_hold_flag;                   // 定点状态
+    char alt_hold_flag;                   // 定高状态
+    char lock_reason_flag;                // 上锁原因
+    char rth_reason_flag;                 // 返航原因
+    char loiter_reason_flag;              // 悬停原因
+    char warn_flag;                       // 报警标志位
+    char gps_status_flag;                 // gps状态
+    char gps_status2_flag;           // 气压计状态
+    char acc_gyro_status_flag;            // 陀螺加计状态
+    char mag_status_flag;                 // 磁状态
+    char flight_mode;                     // 飞行模式
+    uint32_t gps_time_day_hour_minute_second; // gps 时间 时分秒 [时 * 10000 + 分 *
+                                          // 100 + 秒]
+    short gps_time_year_month;   // gps 时间 年月  [(年%100) * 100 + 月]
+    short tlidar_alt;            // 目标雷达高度
+    unsigned char rec_stx;       // 计数累计
+    short data_rec_time;         // 数据记录时间
+    short pid_r;                 // 横滚pidout
+    short pid_p;                 // 俯仰pidout
+    short pid_y;                 // 航向pidout
+    short pid_t;                 // 油门pidout
+    short x_gyro_t;              // 俯仰目标角速度
+    short y_gyro_t;              // 横滚目标角速度
+    short z_gyro_t;              // 航向目标角速度
+    short gps_vertical_vel;      // GPS 垂直速度
+    short raw_baroalt;           // 原始 气压高度
+    char imu_fileter_reset_flag; // imu 滤波器重置标志
+    char imu_mag_yaw;            // imu 收到解锁标志
+    char ahrs_roll;              // ahrs roll
+    char ahrs_pitch;             // ahrs pitch
+    char g0;                     // 标定的重力加速度
+    char redundant_gps_num;      // 冗余 gps 星数
+    char dgps_ant2_gps_num;      // 双天线定向中 ant2 卫星数目
+    int auxiliary_gps_lon;       // 从 gps 经度
+    int auxiliary_gps_lat;       // 从 gps 纬度
+    char ins_error; // ins 姿态正确性 bit0  0-ins 姿态正确 1-ins 姿态异常
+    char vehicle_data_flag;   // 车载数据状态位
+    int16_t vehicle_vector[3];
+    int16_t raw_vehicle_vector[3];
+    uint8_t servo_rpm[8];    // 电调反馈转速
+} FLY_DATA;
+#pragma pack()
+
+void data_record_service(void);
+void put_to_dataflash_buff(unsigned char *src_buff, unsigned short size);
+
+// void flash_write_dataendpage(void);
+// void gsprot_send_datasum_data(void);
+
+// void gsprot_send_datacontent_data(short data_page);
+
+// unsigned int gs_port_send_datasum_data(void);
+
+// void gs_port_send_datalog_data(unsigned int data_pack_num);
+
+void get_fly_data_type_data_5hz(void);
+
+void sd_card_init_fly_log(void);
+void sd_card_clear_fly_log(void);
+
+// extern unsigned char flash_data_translate;
+
+#endif

controller_yy_app/controlware/control_inc/flight_mode.h

@@ -0,0 +1,116 @@
+
+#ifndef __FLIGHT_MODE_H
+#define __FLIGHT_MODE_H
+
+#include "common.h"
+#include "stdbool.h"
+
+enum
+{
+    // 遥控器控制
+    CONTROL_REMOTE = 1,
+    // 地面站控制
+    CONTROL_GCS = 2
+};
+
+enum
+{
+    //===================================遥控器控制下的模式===============================================
+    // 初始化模式，此模式下无任何控制
+    MODE_DEFAULT = 0,
+
+    // 手动速率模式，油门手动，陀螺控制
+    MANUAL_RATE = 1,
+
+    // 手动姿态模式，油门手动，角度控制
+    MANUAL_ATTITUDE = 2,
+
+    // 姿态模式，油门自动，角度控制
+    ATTITUDE = 3,
+
+    // GPS悬停模式，油门自动，定点控制
+    GPS_POSHOLD = 4,
+
+    // GPS无机头模式，油门自动，定点控制
+    GPS_FREEHEAD = 5,
+
+    // GPS环绕模式，油门自动，定点控制
+    GPS_CIRCLE = 6,
+
+    // GPS返航模式，油门自动，定点控制
+    GPS_RTH = 7,
+
+    //===================================地面站控制下的模式===============================================
+    // 摇杆姿态模式
+    ROCK_ATTITUDE = 8,
+
+    // 摇杆定点模式
+    ROCK_POSHOLD = 9,
+
+    // 一键起飞
+    GCS_LAUNCH = 10,
+
+    // GCS悬停模式
+    GCS_LOITER = 11,
+
+    // GCS返航模式
+    GCS_RTH = 12,
+
+    // GCS去第一点
+    GCS_FIRSTPOINT = 13,
+
+    // GCS断点续飞
+    GCS_BREAKPOINT = 14,
+
+    // GCS自动航线
+    GCS_AUTO = 15,
+
+    // GCS继续航线模式
+    GCS_AUTOCONTINUE = 16,
+
+    // GCS指点引导
+    GCS_GUIDETMP = 17,
+
+    // GCS指点模式
+    GCS_GUIDE = 18,
+
+    // GPS降落模式
+    GPS_LAND = 19,
+
+    // 紧急降落
+    FORCE_LAND = 20,
+
+    // GCS 跟随模式
+    GCS_FOLLOW = 21,
+
+    // GCS 低空抛物悬停
+    GCS_LOWLOITER = 22,
+
+    // gcs 航点环绕
+    GCS_WPCIRCLE = 23,
+
+    // 车载跟车起飞
+    GCS_VEHICLE_LAUNCH = 24,
+
+    // 车载跟车降落
+    GCS_VEHICLE_LAND = 25,
+
+    // 自主避障模式
+    GCS_OBAVOID = 26,
+
+    // afc 控制模式
+    GCS_AFC_CONTROL = 50,
+};
+
+extern unsigned char control_mode;
+extern unsigned char flight_mode;
+extern unsigned char flight_mode_flag;
+extern comp_status rock_isenable;
+extern bool battary_volt_islow;
+
+void flight_mode_switch(void);
+
+bool set_flight_mode(unsigned char new_mode);
+void update_flight_mode(void);
+
+#endif

controller_yy_app/controlware/control_inc/forbid_area.h

@@ -0,0 +1,27 @@
+#ifndef __FORBID_AREA_H
+#define __FORBID_AREA_H
+
+#include "geomatry.h"
+
+extern float jfq_min_dist;
+extern float jfq_min_bearing;
+extern unsigned char forbidpath_num;
+extern unsigned char inspath_sub;
+
+extern Coords2d inspath[6];
+extern Coords3d forbid_startendwp[2];
+
+extern Coords2d poly[4], poly_cen;
+
+extern Coords3d inp[2];
+extern Coords3dint intinp[2];
+
+extern Coords3d sp;
+extern Coords2dint intsp;
+
+extern Coords2d pc;
+extern Coords2dint intpc;
+
+void forbid_fly_area_limit(void);
+
+#endif

controller_yy_app/controlware/control_inc/geomatry.h

@@ -0,0 +1,84 @@
+#ifndef GEOMATRY_H
+#define GEOMATRY_H
+
+#include <stdbool.h>
+
+//二维坐标或向量
+typedef struct
+{
+    float x;
+    float y;
+} Coords2df;
+
+//二维坐标或向量
+typedef struct
+{
+    int x;
+    int y;
+} Coords2dint;
+
+//三维坐标或向量
+typedef struct
+{
+    float x;
+    float y;
+    float z;
+} Coords3df;
+
+//三维坐标或向量
+typedef struct
+{
+    int x;
+    int y;
+    int z;
+} Coords3dint;
+
+typedef struct
+{
+    double x;
+    double y;
+} Coords2ddouble;
+
+typedef struct
+{
+    double x;
+    double y;
+    double z;
+} Coords3ddouble;
+
+typedef Coords2df Vector2df;
+typedef Coords2ddouble Vector2ddouble;
+typedef Coords3df Vector3df;
+typedef Coords3ddouble Vector3ddouble;
+typedef Coords2dint Vector2dint;
+typedef Coords3dint Vector3dint;
+
+// 两直线相交
+Coords2df lineInterLine(const Vector3ddouble *lineParama,
+                        const Vector3ddouble *lineParamb);
+
+// 两点求直线方程
+Vector3ddouble calLineEquation(const Coords2ddouble *pa,
+                               const Coords2ddouble *pb);
+
+// 求点是否在多边行内
+bool PointInPolygon(const Coords3dint *point, const Coords3dint *polygon,
+                    int count);
+
+// 求二维平面点到线段的最短距离
+void PointToSegmentDist(const Coords2dint *p, const Coords2dint *segment_a,
+                        const Coords2dint *segment_b, float *dist,
+                        float *angle);
+
+// 求点到多边形的最短距离
+void PointToPolygonDist(const Coords3dint *p, const Coords3dint *polygon,
+                        int count, float *dist, float *angle);
+
+void Cal_LngLat_To_FlatM_Ratio(double lat, double lng, double *ratiox,
+                               double *ratioy);
+
+void LanLat_To_Flat_Dist_Bearing(int lon1, int lat1, int lon2, int lat2,
+                                 double lonToM, double latToM, float *dist_m,
+                                 float *bearing_rad);
+
+#endif

controller_yy_app/controlware/control_inc/lowpass_filter.h

@@ -0,0 +1,35 @@
+
+#ifndef __LOWPASS_FILTER_H
+#define __LOWPASS_FILTER_H
+
+#include "stdbool.h"
+
+#ifndef RATE_RP_FILT_HZ
+#define RATE_RP_FILT_HZ 30.0f
+#endif
+
+#ifndef RATE_YAW_FILT_HZ
+#define RATE_YAW_FILT_HZ 30.0f
+#endif
+
+#ifndef VEL_XY_FILT_HZ
+#define VEL_XY_FILT_HZ 5.0f
+#endif
+
+#ifndef ACCEL_Z_FILT_HZ
+#define ACCEL_Z_FILT_HZ 20.0f
+#endif
+
+float get_filt_alpha(float _filt_hz, float _dt);
+float apply(float sample, float sample_last, float cutoff_freq, float dt);
+void set_input_filter_d(float input, float filt_hz, float dt, float *input_last,
+                        float *d_item, float *d_item_last,
+                        bool *filter_ready_flag);
+float set_input_filter_all(float input, float filt_hz, float dt,
+                           float *input_last, float *d_item,
+                           bool *filter_ready_flag);
+void set_input_filter_me(float err_cur, float filt_hz, float dt,
+                         float d_item_value, float *err_last, float *d_item,
+                         float *d_item_last, bool *filter_ready_flag);
+
+#endif

controller_yy_app/controlware/control_inc/my_math.h

@@ -0,0 +1,84 @@
+
+#ifndef __MY_MATH_H
+#define __MY_MATH_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define M_PI_F 3.14159265358979f //圆周率
+
+#ifndef M_2PI_F
+#define M_2PI_F (2 * M_PI_F)
+#endif
+
+#define EARTH_R 6378100 //地球半径（m）
+
+// acceleration due to gravity in m/s/s
+#define GRAVITY_MSS 9.80665f
+// acceleration due to gravity in cm/s/s
+#define GRAVITY_CMSS 980.665f
+
+// Single precision conversions
+#define DEG_TO_RAD 0.017453292519943295769236907684886f // 度换算弧度
+#define RAD_TO_DEG 57.295779513082320876798154814105f   // 弧度换算度
+
+// convert a longitude or latitude point to meters or centimeteres.
+// Note: this does not include the longitude scaling which is dependent upon
+// location
+#define LONLAT_TO_M 0.01113195f
+#define LONLAT_TO_CM 1.113195f
+
+#define max(a, b)                                                              \
+    ({                                                                         \
+        typeof(a) _a = (a);                                                    \
+        typeof(b) _b = (b);                                                    \
+        _a > _b ? _a : _b;                                                     \
+    })
+
+#define min(a, b)                                                              \
+    ({                                                                         \
+        typeof(a) _a = (a);                                                    \
+        typeof(b) _b = (b);                                                    \
+        _a < _b ? _a : _b;                                                     \
+    })
+
+void buf2float(float *tfloat, unsigned char *buf);
+void buf2long(long *tfloat, unsigned char *buf);
+void buf2int(int *tint, unsigned char *buf);
+void buf2short(short *tshort, unsigned char *buf);
+void float2buf(unsigned char *buf, float *tfloat);
+void int2buf(unsigned char *buf, int *tint);
+void short2buf(unsigned char *buf, short *tshort);
+int brinv(float a[], int n);
+
+// constrain a int16_t value
+int16_t constrain_int16(int16_t amt, int16_t low, int16_t high);
+// constrain a int32_t value
+int32_t constrain_int32(int32_t amt, int32_t low, int32_t high);
+
+unsigned char constrain_uint8(unsigned char value, unsigned char min,
+                              unsigned char max);
+short math_constrain(short value, short min, short max);
+
+short abs_int16(short value);
+
+float min_float(float value1, float value2);
+int min_int32(int value1, int value2);
+short min_int16(short value1, short value2);
+float max_float(float value1, float value2);
+int max_int32(int value1, int value2);
+short max_int16(short value1, short value2);
+
+bool is_zero(const float fVal1);
+
+float no_zero_float(float Val1);
+int no_zero_int32(int Val1);
+
+float sq(float v);
+float get_norm(float a, float b);
+
+// float stdev(short num[], int n);
+
+void insert_sort(int arr[], int n);
+
+#endif

controller_yy_app/controlware/control_inc/pilot_init.h

@@ -0,0 +1,12 @@
+
+#ifndef __PILOT_INIT_H
+#define __PILOT_INIT_H
+
+#include "stdint.h"
+
+
+void auto_pilot_init(void);
+
+#endif
+
+

controller_yy_app/controlware/control_inc/pilot_navigation.h

@@ -0,0 +1,126 @@
+
+#ifndef __PILOT_NAVIGATION_H
+#define __PILOT_NAVIGATION_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define MODE_COORDINATE 1 // 高速过弯
+#define MODE_LOITERTURN 2 // 悬停转弯
+#define MODE_LOCKHEAD 3   // 机头锁定
+#define MODE_THROWING 4   // 抛物
+#define MODE_LANDING 5    // 降落
+#define MODE_CIRCLE 6     // 环绕
+#define MODE_AUTOTURN 7   // 自动转弯
+#define MODE_EXITWP 10
+
+#define TASK_NO 1             // 无任务
+#define TASK_PHOTO_DISTANCE 2 // 启动定距离拍照
+#define TASK_PHOTO_TIME 3     // 启动定时间拍照
+
+#define ARRIVE_POINT_RADIUS 250.0f
+
+#define vector2(TYPE) \
+    typedef struct    \
+    {                 \
+        TYPE x;       \
+        TYPE y;       \
+    } vector2_##TYPE
+
+vector2(float);
+vector2(double);
+vector2(int);
+vector2(short);
+
+#define vector3(TYPE) \
+    typedef struct    \
+    {                 \
+        TYPE x;       \
+        TYPE y;       \
+        TYPE z;       \
+    } vector3_##TYPE
+
+vector3(float);
+vector3(double);
+vector3(int);
+vector3(short);
+
+struct rec_pos
+{
+    int lng;
+    int lat;
+    int alt;
+    int gps_alt;
+    int16_t yaw;
+};
+extern struct rec_pos home_position;
+extern struct rec_pos takeoff_position;
+extern struct rec_pos poshold_position;
+extern struct rec_pos wpphoto_position;
+extern struct rec_pos circlecenter_position;
+extern struct rec_pos target_yaw_lock_position;
+
+extern uint16_t wp_cycle_times;
+extern uint16_t wp_have_cycle_times;
+
+void record_position(int *longitude, int *latitude, int lng, int lat);
+
+float earth_longitude_scale(int lat);
+int point_to_point_distance(int lon1, int lat1, int lon2, int lat2);
+
+float cal_tar_vel_z(int t_alt, int c_alt, int acc_up, int acc_down);
+
+
+//航点数据结构体
+struct waypoint_data
+{
+    unsigned short wp_num;        // 航点序号
+    int wp_lng;                   // 航点经度 *10^7
+    int wp_lat;                   // 航点纬度 *10^7
+    int wp_alt;                   // 航点高度 cm
+    short wp_vel;                 // 巡航速度 cm/s
+    unsigned char wp_mode;        // 航点模式
+    unsigned short wp_mode_param; // 模式参数。cm、s、
+    unsigned char wp_task;        // 航线任务
+    unsigned short wp_task_param; // 任务参数。cm、s、
+    unsigned short wp_totalnum;   // 航点总数
+    short wp_param3;              // 航点机头指向
+    uint8_t wp_alt_type;          // 航点高度类型
+    int16_t wp_param4;            // 环绕半径 dm
+};
+
+
+//航点总数
+extern unsigned short waypoint_totalnum;
+
+//目标航点序号
+extern unsigned short tar_wp_no;
+
+//前一个点到目标点的距离 cm
+extern int wp_prevtotar_distance;
+//前一个点到目标点的方位角 deg
+extern float wp_prevtotar_bearing;
+
+//前一个点到当前点的距离 cm
+extern int wp_prevtocur_distance;
+//前一个点到当前点的方位角 deg
+extern float wp_prevtocur_bearing;
+
+extern int wp_curtotar_distance;
+extern float wp_curtotar_bearing;
+//开始点到当前点的垂线距离
+extern int wp_prevtocur_verdistance;
+//当前点到目标点的垂线距离
+extern int wp_curtotar_verdistance;
+
+extern bool fly_point_flag, update_point_flag, execute_command_flag;
+
+extern unsigned int arrive_point_time;
+
+
+extern float wp_curtotar_bearing_last;
+
+extern bool reset_wp_start_time_flag;
+int cal_tar_vel_xy_unac(int h_dist, int r_dist, int min_vel, int max_vel);
+
+#endif

controller_yy_app/controlware/control_rate.c

@@ -9,7 +9,7 @@
 #include "soft_rc_input.h"
 #include "soft_time.h"
 #include "soft_imu.h"
-#include "control_rate.h"
+#include <control_rate.h>
 #include <math.h>
 
 // malloc测试，定义结构体指针的话必须要malloc申请空间，

controller_yy_app/controlware/control_throttle.c

@@ -435,7 +435,7 @@ void pilot_lock_decide(void)
 通过遥控器的值获取期望爬升速度
 */
 char althold_state = NO_ALT_HOLD;
-
+extern float thr_max;
 // 有个±5的余量，避免垂直速度方向有偏差导致小速度时无法正确执行。15太大了。
 void get_pilot_desired_climb_rate_fromrc(short thr_in)
 {
@@ -452,7 +452,14 @@ void get_pilot_desired_climb_rate_fromrc(short thr_in)
         pid_m_alt.vel_t = 0;
         return;
     }
-
+    // 8通 且遥控器健康
+    if(rc_in[RC_CH8] > 1500 && comp_rc_status == COMP_NORMAL && rc_signal_health == RC_SIGNAL_HEALTH)
+    {
+        pid_m_alt.vel_t = 150; // 期望高度等于当前高度+
+        althold_state = NO_ALT_HOLD;
+        thr_max = 1700.0f;
+        return;
+    }
     // check throttle is above, below or in the deadband
     if (thr_in < 1500 - RC_DEAD_ZONE)
     {

controller_yy_app/controlware/data_save.c

@@ -0,0 +1,558 @@
+#include "data_save.h"
+#include "auto_pilot.h"
+#include "control_rate.h"
+#include "control_throttle.h"
+#include "flight_mode.h"
+#include "gcs_vklink_v30.h"
+#include "mode_attitude.h"
+#include "my_crc.h"
+#include "my_math.h"
+#include "params.h"
+#include "pilot_navigation.h"
+#include "soft_can_yy.h"
+#include "soft_delay.h"
+#include "soft_flash.h"
+#include "soft_gs.h"
+#include "soft_imu.h"
+#include "soft_motor_output.h"
+#include "soft_port_uart4.h"
+#include "soft_rc_input.h"
+#include "soft_sdcard.h"
+#include "soft_time.h"
+#include "soft_usharpradar.h"
+#include "soft_voltage.h"
+#include "ver_config.h"
+#include "vklink.h"
+#include "mode_gcs_tax_launch_run.h"
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+// 定义用来存储数据的buff的个数
+#define DF_BUF_NUM 2
+// 用来定义临时存储DF数据的缓存大小
+#define DF_BUF_SIZE 512
+
+// 打开记录的标志位
+bool data_record_open = false;
+// 结束记录的标志位
+bool data_record_close = false;
+
+// 存储DF(dataflash)数据的 缓存buff
+unsigned char dataflash_buff[DF_BUF_NUM][DF_BUF_SIZE];
+// 选择哪个BUFF的标志位，buff的下标
+unsigned char flag_choice_dfbuff = 0;
+// 从buff的哪个位置开始操作的索引标识符。
+unsigned short cpy_dataflash_index = 0;
+// 数据是否可以写入flash了标志位，，，dataflash_buff存储满了后置位
+unsigned char dfbuff_writo_flash_flag = 0;
+
+// 记录飞行LOG的结构体
+FLY_LOG fly_log = {0};
+
+static VKlink_Msg_Type record_msg = {.head = 0xFE};
+
+////==========================实现函数=============================================
+// 函数原型:		void switch_df_buff(void)
+// 功　　能:	    buff转换，当一个buff写满后，写另一个buff
+//=================================================================================
+void switch_df_buff(void)
+{
+    // dataflash_buff[0]存放满了
+    if (flag_choice_dfbuff == 0)
+    {
+        // 换用dataflash_buff[1]来存放
+        flag_choice_dfbuff = 1;
+        // 把第一个buff写入flash
+        dfbuff_writo_flash_flag = 1;
+    }
+    // dataflash_buff[1]存放满了
+    else if (flag_choice_dfbuff == 1)
+    {
+        // 换用dataflash_buff[0]来存放
+        flag_choice_dfbuff = 0;
+        // 把第二个buff写入flash
+        dfbuff_writo_flash_flag = 2;
+    }
+}
+
+/**
+ * @brief 将数据放入双缓冲内存块
+ *
+ * @param src_buff 待放入缓冲区的数据
+ * @param size 数据的大小字节数
+ */
+void put_to_dataflash_buff(unsigned char *src_buff, unsigned short size)
+{
+    // 可以memcpy的部分的大小，也就是dataflash_buff还剩余的最后部分空间
+    unsigned short valid_cpy_size;
+    // 剩余部分的字节数，存进下一个dataflash_buff
+    unsigned short surplus_cpy_size;
+
+    // 如果dataflash_buff的大小够存放size的大小，直接存放
+    if ((DF_BUF_SIZE - cpy_dataflash_index) >= size)
+    {
+        memcpy((unsigned char *)&dataflash_buff[flag_choice_dfbuff]
+                                               [cpy_dataflash_index],
+               src_buff, size);
+        cpy_dataflash_index += size;
+        // 如果刚好存放满，则下一次换用另一个dataflash_buff，并从开头位置cpy
+        if (cpy_dataflash_index == DF_BUF_SIZE)
+        {
+            switch_df_buff();
+            cpy_dataflash_index = 0;
+        }
+    }
+    // 如果dataflash_buff的大小不够存放size的大小
+    else
+    {
+        // 先cpy  dataflash_buff中能有效存放的字节
+        valid_cpy_size = DF_BUF_SIZE - cpy_dataflash_index;
+        memcpy((unsigned char *)&dataflash_buff[flag_choice_dfbuff]
+                                               [cpy_dataflash_index],
+               src_buff, valid_cpy_size);
+        // 换用另一个dataflash_buff存放数据
+        switch_df_buff();
+        // 从头开始放。
+        cpy_dataflash_index = 0;
+        // 上一次剩余的字节大小
+        surplus_cpy_size = size - valid_cpy_size;
+        // 这里犯过错误，，这一次的cpy应该是复制剩余的部分，从&src_buff[valid_cpy_size]开始复制
+        memcpy((unsigned char *)&dataflash_buff[flag_choice_dfbuff]
+                                               [cpy_dataflash_index],
+               &src_buff[valid_cpy_size], surplus_cpy_size);
+        // 下一次要存放的位置索引
+        cpy_dataflash_index = size - valid_cpy_size;
+    }
+}
+
+uint32_t datasave_time = 0, datasave_timeinterval = 0;
+
+/**
+ * @brief 记录参数
+ *
+ */
+static void record_params(void)
+{
+    static uint8_t record_buff[320];
+    uint8_t tmpUint8 = 0;
+    uint32_t tmpInt = 0;
+    VKlink_Msg_Type *msg = &record_msg;
+    uint32_t record_len = 0;
+    struct waypoint_data wp_temp;
+
+    /*------------- 记录 VER 参数 ----------------------*/
+    msg->payload_len = 0;
+    msg->msgid = GCS_VKLINK_V300_VER_ID;
+    vklink_msg_payload_put_data(msg, verinf._ap_name, 10);
+    vklink_msg_payload_put_data(msg, &verinf._serial_id, 4);
+    tmpInt = ins.imu_conf.version;
+    vklink_msg_payload_put_data(msg, &tmpInt, 4);
+    tmpInt = FW_VER;
+    vklink_msg_payload_put_data(msg, &tmpInt, 4);
+    vklink_msg_payload_put_data(msg, &tmpUint8, 1);
+    vklink_msg_payload_put_data(msg, &tmpUint8, 1);
+    vklink_msg_payload_put_data(msg, &verinf._sysid, 1);
+    record_len = VKlink_MsgTxFormat(msg, record_buff);
+    res_sd = f_write(&fnew_data, record_buff, record_len, &fwnum);
+
+    /*------------- 记录设置参数 -----------------------*/
+    msg->payload_len = 0;
+    msg->msgid = GCS_VKLINK_V300_CONFINF_TOTAL_ID;
+    for (uint16_t i = ParamNum_APType; i < ParamNum_MaxNum; ++i)
+    {
+        uint16_t value = params_get_value(i);
+        vklink_msg_payload_put_data(msg, &value, 2);
+    }
+    record_len = VKlink_MsgTxFormat(msg, record_buff);
+    res_sd = f_write(&fnew_data, record_buff, record_len, &fwnum);
+
+    /*------------- 记录航点信息 -----------------------*/
+    if (waypoint_totalnum)
+    {
+        for (uint16_t i = 1; i <= waypoint_totalnum; ++i)
+        {
+            msg->payload_len = 0;
+            msg->msgid = GCS_VKLINK_V300_WP_ID;
+            flash_read_bytes(WAYPOINT_ADDR + i * sizeof(wp_temp),
+                             (uint8_t *)&wp_temp, sizeof(wp_temp));
+            vklink_msg_payload_put_data(msg, &wp_temp.wp_num, 2);
+            vklink_msg_payload_put_data(msg, &wp_temp.wp_lng, 4);
+            vklink_msg_payload_put_data(msg, &wp_temp.wp_lat, 4);
+            vklink_msg_payload_put_data(msg, &wp_temp.wp_alt, 4);
+            vklink_msg_payload_put_data(msg, &wp_temp.wp_vel, 2);
+            vklink_msg_payload_put_data(msg, &wp_temp.wp_mode, 1);
+            vklink_msg_payload_put_data(msg, &wp_temp.wp_mode_param, 2);
+            vklink_msg_payload_put_data(msg, &wp_temp.wp_task, 1);
+            vklink_msg_payload_put_data(msg, &wp_temp.wp_task_param, 2);
+            vklink_msg_payload_put_data(msg, &wp_temp.wp_param3, 2);
+            vklink_msg_payload_put_data(msg, &wp_temp.wp_alt_type, 1);
+            vklink_msg_payload_put_data(msg, &wp_temp.wp_totalnum, 2);
+
+            record_len = VKlink_MsgTxFormat(msg, record_buff);
+            res_sd = f_write(&fnew_data, record_buff, record_len, &fwnum);
+        }
+    }
+}
+
+/**
+ * @brief 将缓冲区的数据写入 SD 卡
+ *
+ */
+void dfbuff_writo_sdcard_function(void)
+{
+    // 解锁开始记录，解锁之前数据一直在存储。
+    if (data_record_open == true && dfbuff_writo_flash_flag != 0)
+    {
+
+        datasave_time = micros();
+        res_sd = f_write(
+            &fnew_data,
+            (unsigned char *)&dataflash_buff[dfbuff_writo_flash_flag - 1][0],
+            512, &fwnum);
+        res_sd = f_sync(&fnew_data);
+        // 监测写flash时间
+        datasave_timeinterval = micros() - datasave_time;
+        // 清零必须放在后面，因为前面的运算有用到这个变量。。
+        dfbuff_writo_flash_flag = 0;
+    }
+    // 如果停止记录，需要先把整页的写完，再来写剩余的
+    else if ((data_record_open == true) && (data_record_close == true))
+    {
+        // 把buff中不足512字节的部分写入flash,,,读取出来后最后的一部分是以前部分。不需要用。。。
+        if (cpy_dataflash_index != 0)
+        {
+
+            // 清空多余的数据
+            memset((unsigned char *)&dataflash_buff[flag_choice_dfbuff]
+                                                   [cpy_dataflash_index],
+                   0, DF_BUF_SIZE - cpy_dataflash_index);
+            // 打开文件写入数据和页数，写完关闭文件
+            res_sd =
+                f_write(&fnew_data,
+                        (unsigned char *)&dataflash_buff[flag_choice_dfbuff][0],
+                        512, &fwnum);
+            res_sd = f_sync(&fnew_data);
+            flag_choice_dfbuff = 0;
+            cpy_dataflash_index = 0;
+        }
+
+        record_params();
+        res_sd = f_sync(&fnew_data);
+        res_sd = f_close(&fnew_data);
+        // 关闭文件记录，清零总包数，写标志位
+        data_record_open = false;
+        data_record_close = false;
+    }
+}
+
+/**
+ * @brief 初始化flylog结构体，解锁时调用一次
+ */
+void fly_log_struct_record_init(void)
+{
+    fly_log.sorties += 1;
+    fly_log.fireware_num = FW_VER;
+    fly_log.unlock_lon = pid_m_posx.loc_c;
+    fly_log.unlock_lat = pid_m_posy.loc_c;
+    fly_log.plane_type = conf_par.jixing;
+    fly_log.unlock_voltage = Voltage_GetVolt(Volt_MC_CH) * 10;
+    fly_log.unlock_time_year = ins.gps_year % 100;
+    fly_log.unlock_time_month = ins.gps_month;
+    fly_log.unlock_time_day = ins.gps_day;
+    // 转为北京时间
+    fly_log.unlock_time_hour = ins.gps_hour;
+    fly_log.unlock_time_minut = ins.gps_minute;
+    fly_log.unlock_time_second = ins.gps_second;
+
+    fly_log.flight_time = 0;
+    fly_log.flight_mileages = 0.0f;
+    fly_log.min_auto_gps_num = 100;
+    fly_log.max_auto_gps_num = 0;
+    fly_log.max_auto_climb_speed = 0;
+    fly_log.max_auto_down_speed = 0;
+    fly_log.max_auto_horizonal_speed = 0;
+    fly_log.max_auto_pitch = 0;
+    fly_log.max_auto_roll = 0;
+    fly_log.acc_gyro_error = 0;
+    fly_log.gps_error = 0;
+    fly_log.baro_error = 0;
+    fly_log.lidar_error = 0;
+    fly_log.mag_error = 0;
+}
+
+/**
+ * @brief 数据记录服务函数，轮训调用
+ *
+ */
+void data_record_service(void)
+{
+    // 数据记录进行的时间，用于上锁后延迟记录
+    static uint32_t data_record_time = 0;
+
+    char file_name[13] = "", logfile_path[50] = "";
+    // 解锁开始记录
+    if (thr_lock_status == UNLOCKED || flight_mode == GCS_VEHICLE_LAUNCH)
+    {
+        if (data_record_open != true)
+        {
+            // 解锁赋予fly_log结构体部分信息，并写一条新的log
+            fly_log_struct_record_init();
+
+            res_sd = f_open(&fnew_log, "LOG/LOG_FLY.DAT", FA_READ | FA_WRITE);
+            res_sd = f_lseek(&fnew_log, f_size(&fnew_log));
+            res_sd = f_write(&fnew_log, &fly_log, sizeof(fly_log), &fwnum);
+            res_sd = f_close(&fnew_log);
+
+            // 根据飞行架次数，增删fly_data文件
+            if (fly_log.sorties > 50)
+            {
+                sprintf(file_name, "%d", fly_log.sorties - 50);
+                strcat(logfile_path, "LOG/");
+                strcat(logfile_path, file_name);
+                res_sd = f_unlink(logfile_path);
+                logfile_path[0] = '\0';
+            }
+            // 拼接文件路径
+            sprintf(file_name, "%d", fly_log.sorties);
+            strcat(logfile_path, "LOG/");
+            strcat(logfile_path, file_name);
+
+            res_sd =
+                f_open(&fnew_data, logfile_path, FA_CREATE_ALWAYS | FA_WRITE);
+
+            // 不断电持续记录。。还是解锁从头记录?
+            data_record_open = true;
+            data_record_close = false;
+        }
+
+        data_record_time = micros();
+    }
+
+    // 上锁关闭记录,延时100ms多记录一包数据。延时不宜过大，有些飞的矮的掉下来电池直接断电了导致最后最后一页的数据没记录上。
+    if (thr_lock_status == LOCKED && data_record_open == true &&
+        micros() - data_record_time > 100000 && flight_mode != GCS_VEHICLE_LAUNCH)
+    {
+        // 上锁再赋值一下fly_log结构体中的部分数据，并再次写入文件
+        fly_log.lock_lon = pid_m_posx.loc_c;
+        fly_log.lock_lat = pid_m_posy.loc_t;
+        fly_log.lock_voltage = Voltage_GetVolt(Volt_MC_CH) * 10;
+        fly_log.flight_time = Get_HaveFlyTimeThisSort() / 60.0f;
+        fly_log.total_flight_time += fly_log.flight_time;
+        fly_log.flight_mileages = 0;
+        fly_log.total_flight_mileages += fly_log.flight_mileages;
+
+        res_sd = f_open(&fnew_log, "LOG/LOG_FLY.DAT", FA_READ | FA_WRITE);
+        res_sd = f_lseek(&fnew_log, f_size(&fnew_log) - sizeof(fly_log));
+        res_sd = f_write(&fnew_log, &fly_log, sizeof(fly_log), &fwnum);
+        res_sd = f_close(&fnew_log);
+        // 上锁之前发送一包IMU数据。正常上锁记录的数据零数据之前最后一包肯定是IMU数据。
+        get_fly_data_type_data_5hz();
+
+        data_record_close = true;
+    }
+
+    // buff数据写入FLASH
+    // dfbuff_writo_flash_function();
+    dfbuff_writo_sdcard_function();
+}
+
+/**
+ * @brief
+ * 从sd卡的LOG/LOG_FLY.DAT文件中，读取最后一条飞行日志，以获取总架次，总时长等信息
+ */
+void sd_card_init_fly_log(void)
+{
+    if (f_open(&fnew_data, "LOG/LOG_FLY.DAT", FA_READ) == FR_OK)
+    {
+        if (f_size(&fnew_data) >= sizeof(fly_log))
+        {
+            f_lseek(&fnew_data, f_size(&fnew_data) - sizeof(fly_log));
+            f_read(&fnew_data, &fly_log, sizeof(fly_log), &frnum);
+        }
+        f_close(&fnew_data);
+    }
+}
+
+/**
+ * @brief 清除现有的log信息，重新建立一个log文件
+ * @reval none
+ */
+void sd_card_clear_fly_log(void) { sdcard_inital(1); }
+
+/**
+ * @brief 记录飞行数据5hz
+ */
+void get_fly_data_type_data_5hz(void)
+{
+    // 格式封装，计算校验
+    static uint8_t record_buff[6 + sizeof(FLY_DATA) + 2];
+
+    FLY_DATA *fly_data = (FLY_DATA *)&record_buff[6];
+    const struct yy_can_rx_msg_data *yycan_data = yy_can_rx_data_get();
+
+    //----给数据赋值----
+    fly_data->solved_lon = _cxy.pc[0] * 100;
+    fly_data->solved_lat = _cxy.pc[1] * 100;
+    fly_data->target_lon = _cxy.pt[0] * 100;
+    fly_data->target_lat = _cxy.pt[1] * 100;
+    fly_data->raw_lon = raw_gps._auxiliary_gps_lon ; // 外部气压计压力 // raw_gps._longitude;
+    fly_data->raw_lat = raw_gps._auxiliary_gps_lat ; // 外部气压高度 // raw_gps._latitude;
+    fly_data->flight_time = Get_HaveFlyTimeThisSort();
+    fly_data->roll = pid_m_roll.angle_c * 10;
+    fly_data->pitch = pid_m_pitch.angle_c * 10;
+    fly_data->yaw = pid_m_yaw.angle_c * 10;
+    fly_data->target_roll = pid_m_roll.angle_t * 10;
+    fly_data->target_pitch = pid_m_pitch.angle_t * 10;
+    fly_data->target_yaw = pid_m_yaw.angle_t * 10;
+    fly_data->x_gyro = pid_m_pitch.gyro_c * 10;
+    fly_data->y_gyro = pid_m_roll.gyro_c * 10;
+    fly_data->z_gyro = pid_m_yaw.gyro_c * 10;
+    fly_data->x_acc = pid_m_posx.acc_c;
+    fly_data->y_acc = pid_m_posy.acc_c;
+    fly_data->z_acc = pid_m_alt.acc_c;
+
+    fly_data->x_raw_gyro1 = raw_sensor._xgyro1 / 5;
+    fly_data->y_raw_gyro1 = raw_sensor._ygyro1 / 5;
+    fly_data->z_raw_gyro1 = raw_sensor._zgyro1 / 5;
+    fly_data->x_raw_gyro2 = raw_sensor._xgyro2 / 5;
+    fly_data->y_raw_gyro2 = raw_sensor._ygyro2 / 5;
+    fly_data->z_raw_gyro2 = raw_sensor._zgyro2 / 5;
+    fly_data->x_raw_acc1 = raw_sensor._xacc1 * 10;
+    fly_data->y_raw_acc1 = raw_sensor._yacc1 * 10;
+    fly_data->z_raw_acc1 = raw_sensor._zacc1 * 10;
+    fly_data->x_raw_acc2 = raw_sensor._xacc2 * 10;
+    fly_data->y_raw_acc2 = raw_sensor._yacc2 * 10;
+    fly_data->z_raw_acc2 = raw_sensor._zacc2 * 10;
+
+    fly_data->x_raw_mag1 = raw_sensor._xmag1;
+    fly_data->y_raw_mag1 = raw_sensor._ymag1;
+    fly_data->z_raw_mag1 = raw_sensor._zmag1;
+    fly_data->x_raw_mag2 = raw_sensor._xmag2;
+    fly_data->y_raw_mag2 = raw_sensor._ymag2;
+    fly_data->z_raw_mag2 = raw_sensor._zmag2;
+
+    fly_data->air_speed = pid_m_alt.thr_hold;
+    fly_data->solved_alt = pid_m_alt.loc_c / 10;
+    fly_data->target_alt = pid_m_alt.loc_t / 10;
+    fly_data->forward_lidar = record_target_alt / 10;
+    fly_data->backward_lidar = _cxy.at[0] * 100.f; //0 / 10;
+    fly_data->down_lidar_alt = _cxy.at[1] * 100.f; //0 / 10;
+    fly_data->tlidar_alt =  0 ;//0;
+    fly_data->lidar_comp_state = angle_plan_run_cnt ;//0 /10; 角度策略生效次数
+    fly_data->climb_rate = pid_m_alt.vel_c;
+    fly_data->target_climb_rate = pid_m_alt.vel_t;
+    fly_data->east_solved_speed = _cxy.vc[0] * 100;
+    fly_data->north_solved_speed = _cxy.vc[1] * 100;
+    fly_data->target_east_solved_speed = _cxy.vt[0] * 100.f;
+    fly_data->target_north_solved_speed = _cxy.vt[1] * 100.f;
+    fly_data->total_wp_number = waypoint_totalnum;
+    fly_data->target_wp_number = tar_wp_no;
+    fly_data->picture_number = 0;
+    fly_data->voltage_value = Voltage_GetVolt(Volt_MC_CH) * 10;
+    fly_data->dist_2_home = 0;
+    fly_data->dist_2_target = wp_curtotar_verdistance / 100;
+    fly_data->dist_off_track = 0;
+    fly_data->gps_alt = ins.gps_alt / 100;
+    fly_data->gps_yaw = ins.gps_yaw;
+    fly_data->gps_speed = ins.gps_vel;
+    fly_data->gps_num = debug_mode; // ins.gps_num;
+    fly_data->gps_pdop = ins.gps_pdop;
+
+    fly_data->pwmin1 = tmp_rc_in[0] / 10;
+    fly_data->pwmin2 = tmp_rc_in[1] / 10;
+    fly_data->pwmin3 = tmp_rc_in[2] / 10;
+    fly_data->pwmin4 = tmp_rc_in[3] / 10;
+    fly_data->pwmin5 = tmp_rc_in[4] / 10;
+    fly_data->pwmin6 = tmp_rc_in[5] / 10;
+    fly_data->pwmin7 = tmp_rc_in[6] / 10;
+    fly_data->pwmin8 = tmp_rc_in[7] / 10;
+
+    fly_data->pwmout1 = get_motor_pwm(MOTOR1) / 10;
+    fly_data->pwmout2 = get_motor_pwm(MOTOR2) / 10;
+    fly_data->pwmout3 = get_motor_pwm(MOTOR3) / 10;
+    fly_data->pwmout4 = get_motor_pwm(MOTOR4) / 10;
+    fly_data->pwmout5 = _cxy.pos_integ[0] * 10;// get_motor_pwm(MOTOR5) / 10;
+    fly_data->pwmout6 = _cxy.pos_integ[1] * 10;//get_motor_pwm(MOTOR6) / 10;
+    fly_data->pwmout7 = pid_m_pitch.gyro_p_item;//get_motor_pwm(MOTOR7) / 10;
+    fly_data->pwmout8 = pid_m_pitch.gyro_d_item;//get_motor_pwm(MOTOR8) / 10;
+
+    fly_data->temperature = ins.temprature;
+    fly_data->rtk_flag = ins.rtk_state;
+    fly_data->ins_flag =
+        (ins.insgps_ok & 0x0F) + (raw_gps._vehicle_vector_flag << 4);
+    fly_data->lock_flag = thr_lock_status;
+    fly_data->pos_hold_flag = 2;
+    fly_data->alt_hold_flag = althold_state;
+    fly_data->lock_reason_flag = thr_lock_flag;
+    fly_data->rth_reason_flag = 0;
+    fly_data->loiter_reason_flag = 0;
+    fly_data->warn_flag = warn_reason;
+    fly_data->gps_status_flag = raw_gps._gps_status;
+    fly_data->gps_status2_flag = raw_gps._gps_status2;
+    fly_data->acc_gyro_status_flag = raw_gps._acc_gyro_status;
+    fly_data->mag_status_flag = raw_gps._mag_status;
+    fly_data->flight_mode = flight_mode;
+    fly_data->gps_time_year_month = (ins.gps_year % 100) * 100 + ins.gps_month;
+    fly_data->gps_time_day_hour_minute_second =
+        ins.gps_day * 1000000 + ins.gps_hour * 10000 + ins.gps_minute * 100 + ins.gps_second;
+    fly_data->rec_stx++;
+    fly_data->data_rec_time = datasave_timeinterval / 1000;
+    fly_data->pid_r = pid_roll;
+    fly_data->pid_p = pid_pitch;
+    fly_data->pid_y = pid_yaw;
+    fly_data->pid_t = pid_thr;
+    fly_data->x_gyro_t = pid_m_pitch.gyro_t * 10;
+    fly_data->y_gyro_t = pid_m_roll.gyro_t * 10;
+    fly_data->z_gyro_t = pid_m_yaw.gyro_t * 10;
+    fly_data->gps_vertical_vel = raw_gps._gps_vetical_vel / 100;
+
+    fly_data->raw_baroalt = raw_sensor._baroalt * 10;
+    fly_data->imu_fileter_reset_flag = raw_gps._imu_get_throttle_flag;
+    fly_data->imu_mag_yaw = raw_sensor._mag_yaw;
+    fly_data->ahrs_roll = raw_sensor._ahrs_roll;
+    fly_data->ahrs_pitch = raw_sensor._ahrs_pitch;
+    fly_data->g0 = raw_gps._g0;
+    fly_data->redundant_gps_num = status_broadcast;
+    fly_data->dgps_ant2_gps_num = yycan_data->txgl_drone_cmd;
+    fly_data->auxiliary_gps_lon = yycan_data->txgl_alt_sp * 100;    // raw_gps._auxiliary_gps_lon;
+    fly_data->auxiliary_gps_lat = yycan_data->txgl_yaw_sp * 100;   // raw_gps._auxiliary_gps_lat;
+    fly_data->ins_error = raw_gps._ins_error;
+
+    // (vehicleData.status + (vehicleData.vehiclePosLockFlag << 2) +
+    //  (vehicleData.vehicleHeadingLockFlag << 4));
+    // for (size_t i = 0; i < 3; i++)
+    // {
+    //     fly_data->vehicle_vector[i] = vehicleData.currentVector_ENU[i] / 10;
+    // }
+    fly_data->vehicle_vector[0] = pid_m_roll.gyro_i_item /*yycan_data->txgl_enu_vel[0]*/ * 10;
+    fly_data->vehicle_vector[1] = pid_m_pitch.gyro_i_item /*yycan_data->txgl_enu_vel[1]*/ * 10;
+    fly_data->vehicle_vector[2] = yycan_data->txgl_enu_vel[2] * 10;
+
+    fly_data->raw_vehicle_vector[0] = yycan_data->hdw_enu_rpos[0] * 100;
+    fly_data->raw_vehicle_vector[1] = yycan_data->hdw_enu_rpos[1] * 100;
+    fly_data->raw_vehicle_vector[2] = yycan_data->hdw_enu_rpos[2] * 100;
+    fly_data->vehicle_data_flag = yycan_data->hdw_status & 0xff;
+
+    int16_t check_sum = 0, index = 0;
+    size_t len = sizeof(FLY_DATA);
+    if (len >= 255)
+    {
+        len = 255;
+    }
+    record_buff[index++] = 0XFE;
+    record_buff[index++] = len;
+    record_buff[index++] = 0;
+    record_buff[index++] = 0;
+    record_buff[index++] = 0;
+    record_buff[index++] = 0xff;
+
+    index += len;
+
+    check_sum = crc16_cyc_cal(0xFFFF, record_buff, index);
+    record_buff[index++] = check_sum & 0xff;
+    record_buff[index++] = (check_sum >> 8) & 0xff;
+
+    // 记录数据
+    put_to_dataflash_buff(record_buff, index);
+}

controller_yy_app/controlware/flight_mode.c

@@ -0,0 +1,164 @@
+#include "flight_mode.h"
+#include "auto_pilot.h"
+#include "control_rate.h"
+#include "control_throttle.h"
+#include "mode_attitude.h"
+#include "mode_gcs_tax_launch_run.h"
+#include "mode_vehicle_land.h"
+#include "mode_vehicle_launch.h"
+#include "params.h"
+#include "soft_flash.h"
+#include "soft_gps.h"
+#include "soft_gs.h"
+#include "soft_imu.h"
+#include "soft_motor_output.h"
+#include "soft_rc_input.h"
+#include "soft_time.h"
+#include "soft_voltage.h"
+
+unsigned char control_mode = CONTROL_GCS;
+unsigned char flight_mode = MODE_DEFAULT;
+unsigned char flight_mode_flag = MODE_DEFAULT;
+
+// @brief 飞行模式切换
+void flight_mode_switch(void)
+{
+    static short gcs_remote_switch = 1700;
+    //=============== 如果是遥控器控制	====================
+    if (control_mode == CONTROL_REMOTE)
+    {
+        if (comp_rc_status == COMP_NORMAL &&
+            rc_signal_health == RC_SIGNAL_HEALTH)
+        {
+            // 姿态模式
+            if (rc_in[RC_CH5] >= MID_S && rc_in[RC_CH5] <= MAX_E)
+            {
+                control_mode = CONTROL_REMOTE;
+                set_flight_mode(ATTITUDE);
+            }
+            // GPS模式
+            else if (rc_in[RC_CH5] >= MIN_S && rc_in[RC_CH5] <= MIN_E)
+            {
+                if (ins.insgps_ok == INSGPS)
+                {
+                    control_mode = CONTROL_GCS;
+                    flight_mode_flag = GCS_VEHICLE_LAUNCH;
+
+                    set_flight_mode(flight_mode_flag);
+                     __mode_state = __STATE_LAUNCH_GO;
+                     
+                     pid_m_alt.thr_hold == false;
+                     althold_state = NO_ALT_HOLD;
+                     record_target_alt = pid_m_alt.loc_c; // 需要修改
+                }
+            }
+        }
+        else
+        {
+            if (ground_air_status == ON_GROUND)
+            {
+                // 避免遥控器解锁然后接收机受干扰后飞机一直怠速转无法停转。
+                if (thr_lock_status == UNLOCKED)
+                {
+                    set_thr_lock_status(LOCKED, RCBADLOCK);
+                }
+            }
+        }
+    }
+    // 如果是地面站控制
+    else if (control_mode == CONTROL_GCS)
+    {
+        set_flight_mode(flight_mode_flag);
+
+        // RC开并且RC状态健康
+        if (comp_rc_status == COMP_NORMAL &&
+            rc_signal_health == RC_SIGNAL_HEALTH)
+        {
+            // 地面站模式下  从GPS模式切到姿态模式时  遥控器重新获取控制权
+            if (rc_in[RC_CH5] >= MIN_S && rc_in[RC_CH5] <= MIN_E)
+            {
+                gcs_remote_switch = rc_in[RC_CH5];
+            }
+            else if ((rc_in[RC_CH5] >= MAX_S && rc_in[RC_CH5] <= MAX_E) &&
+                     (gcs_remote_switch >= MIN_S && gcs_remote_switch <= MIN_E))
+            {
+                gcs_remote_switch = rc_in[RC_CH5];
+                control_mode = CONTROL_REMOTE;
+                set_flight_mode(ATTITUDE);
+            }
+        }
+    }
+}
+
+
+// 电压低标志位
+bool battary_volt_islow = false;
+
+// 托盘摇杆是否有效
+comp_status rock_isenable = COMP_NOEXIST;
+
+
+
+// @brief 模式退出
+void exit_cur_mode(unsigned char cur_mode, unsigned char new_mode)
+{
+    switch (cur_mode)
+    {
+    case ATTITUDE:
+        attitude_exit();
+        break;
+    case GCS_VEHICLE_LAUNCH:
+        gcs_tax_launch_exit();
+        break;
+    default:
+        break;
+    }
+}
+
+// @brief 设置飞行模式
+bool set_flight_mode(unsigned char new_mode)
+{
+    bool success = false;
+
+    if (new_mode == flight_mode)
+    {
+        return true;
+    }
+
+    switch (new_mode)
+    {
+    case ATTITUDE:
+        success = attitude_init();
+        break;
+    case GCS_VEHICLE_LAUNCH:
+        success = gcs_tax_launch_init();
+        break;
+    default:
+        success = false;
+        break;
+    }
+
+    if (success)
+    {
+        exit_cur_mode(flight_mode, new_mode);
+        flight_mode = new_mode;
+    }
+
+    return success;
+}
+
+// @brief 模式更新
+void update_flight_mode(void)
+{
+    switch (flight_mode)
+    {
+    case ATTITUDE:
+        attitude_run();
+        break;
+    case GCS_VEHICLE_LAUNCH:
+        gcs_tax_launch_run(fast_loop_dt);
+        break;
+    default:
+        break;
+    }
+}

controller_yy_app/controlware/geomatry.c

@@ -0,0 +1,387 @@
+#include "geomatry.h"
+#include "helpler_funtions.h"
+#include "math.h"
+#include "my_math.h"
+#include "pilot_navigation.h"
+#include "soft_time.h"
+#include <stdlib.h>
+#include <string.h>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+// @brief 给定平面两条直线求交点坐标
+// @params const Vector3d* la //直线
+//         sconst Vector3d* lb //直线
+// @retval Coords2d cp 二维交点的坐标
+Coords2df lineInterLine(const Vector3ddouble *la, const Vector3ddouble *lb)
+{
+    Coords2df cp;
+    double a, b, c, A, B, C;
+    a = la->x;
+    b = la->y;
+    c = la->z;
+    if (b == 0.0f)
+    {
+        c = c / a;
+        a = 1;
+    }
+    else
+    {
+        a = a / b;
+        c = c / b;
+        b = 1;
+    }
+    A = lb->x;
+    B = lb->y;
+    C = lb->z;
+    if (B == 0.0f)
+    {
+        C = C / A;
+        A = 1;
+    }
+    else
+    {
+        A = A / B;
+        C = C / B;
+        B = 1;
+    }
+    // 不平行才有交点
+    if (a != A || b != B)
+    {
+        cp.x = -(b * C - B * c) / (A * b - a * B);
+        cp.y = -(A * c - a * C) / (A * b - a * B);
+    }
+    return cp;
+}
+
+// 函数：求两点直线
+// 功能：给定两个点a\b，求所在直线参数
+// 参数：const Coords2d* pa //点a
+//       const Vector2d* pb //点b
+// 返回：直calLineEquation线方程参数
+Vector3ddouble calLineEquation(const Coords2ddouble *pa,
+                               const Coords2ddouble *pb)
+{
+    double a, b, c;
+    if (pa->x == pb->x)
+    {
+        a = 1;
+        b = 0;
+        c = -pa->x;
+    }
+    else
+    {
+        a = -(double)(pa->y - pb->y) / (double)(pa->x - pb->x);
+        b = 1;
+        c = (double)(pa->y * pb->x - pa->x * pb->y) / (double)(pa->x - pb->x);
+    }
+    Vector3ddouble params;
+    params.x = a;
+    params.y = b;
+    params.z = c;
+    return params;
+}
+
+// 函数：点在直线上的垂直投影
+// 功能：给定一点和一条直线，求该点在该直线上的垂直投影
+// 参数：const Coords2d* p //点p
+//       const Vector3d* lineParam //直线方程参数
+// 返回：投影点坐标
+Coords2df pointProjection2Line(const Coords2df *p, const Vector3ddouble *params)
+{
+    Coords2df pointProjection;
+    //如果点在线上，投影点就是该点
+    if ((int)(params->x * p->x + params->y * p->y + params->z) == 0)
+        pointProjection = *p;
+    else
+    {
+        //通过该点的与给定直线垂直的直线
+        double lineParamTemp[3] = {-params->y, params->x,
+                                   params->y * p->x - params->x * p->y};
+        //两条线的交点就是垂直投影点
+        const Coords3ddouble *param1, *param2;
+        param1 = (Coords3ddouble *)params;
+        param2 = (Coords3ddouble *)lineParamTemp;
+        pointProjection = lineInterLine(param1, param2);
+    }
+    return pointProjection;
+}
+
+// @brief 判断点在多边形内部
+// @param const Coords2dint *point 点坐标指针
+// @param const Coords2dint *polygon 多变形顶点坐标指针
+// @param int count 多边形顶点个数
+// @retval
+bool PointInPolygon(const Coords3dint *point, const Coords3dint *polygon,
+                    int count)
+{
+    int cross = 0;
+
+    int x_max = polygon[0].x, y_max = polygon[0].y;
+    int x_min = polygon[0].x, y_min = polygon[0].y;
+    for (int i = 1; i < count; ++i)
+    {
+        if (polygon[i].x > x_max)
+            x_max = polygon[i].x;
+        if (polygon[i].x < x_min)
+            x_min = polygon[i].x;
+        if (polygon[i].y > y_max)
+            y_max = polygon[i].y;
+        if (polygon[i].y < y_min)
+            y_min = polygon[i].y;
+    }
+    if (point->x > x_max || point->x < x_min || point->y > y_max ||
+        point->y < y_min)
+    {
+        return false;
+    }
+
+    for (int i = 0; i < count; ++i)
+    {
+        const Coords3dint *p1, *p2;
+        p1 = &polygon[i];
+        p2 = &polygon[(i + 1) % count];
+
+        // 以 y = point.y 为射线，求其与多变形的交点个数
+        // p1, p2 连线与 x 轴平行，则为无交点
+        if (p1->y == p2->y)
+            continue;
+        // 交点落在 p1 p2 延长线上
+        else if (point->y < (p1->y < p2->y ? p1->y : p2->y))
+            continue;
+        else if (point->y >= (p1->y > p2->y ? p1->y : p2->y))
+            continue;
+        else
+        {
+            double x;
+            x = (double)(point->y - p1->y) * (double)(p2->x - p1->x) /
+                    (double)(p2->y - p1->y) +
+                p1->x;
+            if (x > point->x)
+            {
+                cross++;
+            }
+        }
+    }
+
+    if (cross % 2)
+        return true;
+    else
+        return false;
+}
+
+// @brief 求二维平面点到线段的最短距离，以及改点到最短距离点的方位角
+// @param *p 点的坐标数据指针
+// @param *segment_a 线段端点 a 的坐标指针
+// @param *segment_b 线段端点 b 的坐标指针
+// @param *dist 点到线段上最近点的距离
+// @param *angle 点到线段上最近点的方位角
+void PointToSegmentDist(const Coords2dint *p, const Coords2dint *segment_a,
+                        const Coords2dint *segment_b, float *dist, float *angle)
+{
+    // 计算向量 AB, AP
+    Vector2dint AB = {.x = segment_b->x - segment_a->x,
+                      .y = segment_b->y - segment_a->y};
+    Vector2dint AP = {.x = p->x - segment_a->x, .y = p->y - segment_a->y};
+
+    // 计算 AB * AP
+    float AB_dot_AP = (float)AB.x * AP.x + (float)AB.y * AP.y;
+
+    // 计算 |AB|^2
+    float AB_dist_squar = (float)AB.x * AB.x + (float)AB.y * (float)AB.y;
+
+    // 如果 AB AP 的点积小于零，说明二者夹角大于 90 deg，PA 为最短距离
+    if (AB_dot_AP < 0.0f)
+    {
+        *dist = get_norm(segment_a->x - p->x, segment_a->y - p->y);
+        *angle = atan2f(segment_a->x - p->x, segment_a->y - p->y) * RAD_TO_DEG;
+    }
+    // 如果 AB AP 的夹角小于 90 deg
+    else
+    {
+        if (AB_dot_AP > AB_dist_squar)
+        {
+            // 如果 AP 在 AB 上的投影超过了 AB，那么最短距离为 PB
+            *dist = get_norm(segment_b->x - p->x, segment_b->y - p->y);
+            *angle =
+                atan2f(segment_b->x - p->x, segment_b->y - p->y) * RAD_TO_DEG;
+        }
+        else
+        {
+            // 如果 AP 在 AB 上的投影在 AB 线段内，则最短距离为 AP 在 AB
+            // 法线方向上的投影
+            // 角度为 AB 方向 +- (pi / 2)
+            float angle_AB = atan2f(segment_b->x - segment_a->x,
+                                    segment_b->y - segment_a->y);
+            float angle_AP = atan2f(p->x - segment_a->x, p->y - segment_a->y);
+
+            float angle_AP_AB = wrap_pi(angle_AB - angle_AP);
+
+            *dist = get_norm(p->x - segment_a->x, p->y - segment_a->y) *
+                    sinf(angle_AP_AB);
+
+            if (angle_AP_AB > 0.0f)
+            {
+                *angle = wrap_pi(angle_AB + M_PI_F / 2.0f) * RAD_TO_DEG;
+            }
+            else
+            {
+                *angle = wrap_pi(angle_AB - M_PI_F / 2.0f) * RAD_TO_DEG;
+            }
+        }
+    }
+}
+
+// @brief 求点到多边形最短距离
+// @param *p 点的坐标数据指针
+// @param *polygon 多边形顶点坐标数据指针
+// @param count 多边形顶点个数，从 1 开始数
+// @param *dist 传出结果，点到多边形的最短距离
+// @param *angle 传出结果，到多边形最近点的方位角，单位 deg
+void PointToPolygonDist(const Coords3dint *p, const Coords3dint *polygon,
+                        int count, float *dist, float *angle)
+{
+    float min_dist = INFINITY, min_dist_angle = 0.0f;
+    for (int i = 0; i < count; ++i)
+    {
+        Coords2dint A = {.x = polygon[i].x, .y = polygon[i].y};
+        Coords2dint B = {.x = polygon[(i + 1) / count].x,
+                         .y = polygon[(i + 1) / count].y};
+        Coords2dint P = {p->x, p->y};
+        float tmp_dist, tmp_angle;
+        PointToSegmentDist(&P, &A, &B, &tmp_dist, &tmp_angle);
+        if (tmp_dist < min_dist)
+        {
+            min_dist = tmp_dist;
+            min_dist_angle = tmp_angle;
+        }
+    }
+
+    *dist = min_dist;
+    *angle = min_dist_angle;
+}
+
+static double deg2rad(double deg) { return deg * M_PI / 180; }
+
+/**
+ * @brief Vincenty 经纬度转平面距离
+ *
+ * @param latA A 点纬度
+ * @param lngA A 点经度
+ * @param latB B 点纬度
+ * @param lngB B 点经度
+ * @return double 距离
+ */
+double distanceVincenty(double latA, double lngA, double latB, double lngB)
+{
+    double b = 6356752.3142;
+    double f = 1 / 298.257223563;
+    const double earthR = 6378137.0;
+
+    double L = deg2rad(lngB - lngA);
+    double U1 = atan((1 - f) * tan(deg2rad(latA)));
+    double U2 = atan((1 - f) * tan(deg2rad(latB)));
+    double sinU1 = sin(U1);
+    double cosU1 = cos(U1);
+    double sinU2 = sin(U2);
+    double cosU2 = cos(U2);
+    double lambda = L;
+    double lambdaP = 2 * M_PI;
+    double iterLimit = 20;
+
+    double sinLambda, cosLambda;
+    double sinSigma = 0;
+    double cosSigma = 0;
+    double sigma = 0;
+    double alpha;
+    double cosSqAlpha = 0;
+    double cos2SigmaM = 0;
+    double C;
+
+    while (fabs(lambda - lambdaP) > 1e-12 && --iterLimit > 0)
+    {
+        sinLambda = sin(lambda);
+        cosLambda = cos(lambda);
+        sinSigma = sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) +
+                        (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) *
+                            (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
+        if (sinSigma == 0)
+        {
+            return 0; // co-incident points
+        }
+        cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
+        sigma = atan2(sinSigma, cosSigma);
+        alpha = asin(cosU1 * cosU2 * sinLambda / sinSigma);
+        cosSqAlpha = cos(alpha) * cos(alpha);
+        cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
+        C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
+        lambdaP = lambda;
+        lambda = L + (1 - C) * f * sin(alpha) *
+                         (sigma + C * sinSigma *
+                                      (cos2SigmaM +
+                                       C * cosSigma *
+                                           (-1 + 2 * cos2SigmaM * cos2SigmaM)));
+    }
+    if (iterLimit == 0)
+    {
+        return 0.0; // formula failed to converge
+    }
+    double uSq = cosSqAlpha * (earthR * earthR - b * b) / (b * b);
+    double A =
+        1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
+    double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
+    double deltaSigma =
+        B * sinSigma *
+        (cos2SigmaM + B / 4 *
+                          (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
+                           B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) *
+                               (-3 + 4 * cos2SigmaM * cos2SigmaM)));
+    return b * A * (sigma - deltaSigma);
+}
+
+/**
+ * @brief 计算经纬度转平面坐标距离的系数
+ *
+ * @param lat 纬度
+ * @param lng 经度
+ * @param ratiox 经度转平面距离m系数
+ * @param ratioy 纬度转平面距离m系数
+ */
+void Cal_LngLat_To_FlatM_Ratio(double lat, double lng, double *ratiox,
+                               double *ratioy)
+{
+    double DIFF = 0.0001;
+    double deltax = distanceVincenty(lat, lng, lat, lng + DIFF);
+    double deltay = distanceVincenty(lat, lng, lat + DIFF, lng);
+    *ratiox = deltax / DIFF;
+    *ratioy = deltay / DIFF;
+}
+
+/**
+ * @brief 根据经纬度计算距离和方位角
+ *
+ * @param lon1 经度1
+ * @param lat1 纬度1
+ * @param lon2 经度2
+ * @param lat2 纬度2
+ * @param lonToM 经度差转平面距离m系数
+ * @param latToM 纬度差转平面距离m系数
+ * @param dist_m 距离m
+ * @param bearing_rad 1->2方位角rad
+ */
+void LanLat_To_Flat_Dist_Bearing(int lon1, int lat1, int lon2, int lat2,
+                                 double lonToM, double latToM, float *dist_m,
+                                 float *bearing_rad)
+{
+    double lon_e = wrap_double(lon2 - lon1, -180.0 * 1e7, 180.0 * 1e7);
+    double lat_e = wrap_double(lat2 - lat1, -180.0 * 1e7, 180 * 1e7);
+    double lon_delta = (double)lon_e * 1e-7;
+    double lat_delta = (double)lat_e * 1e-7;
+
+    float dist_e = lonToM * lon_delta;
+    float dist_n = latToM * lat_delta;
+    *dist_m = get_norm(dist_e, dist_n);
+    *bearing_rad = atan2f(dist_e, dist_n);
+}

controller_yy_app/controlware/lowpass_filter.c

@@ -0,0 +1,133 @@
+
+#include "lowpass_filter.h"
+#include "helpler_funtions.h"
+#include "my_math.h"
+#include <float.h>
+#include <math.h>
+
+
+
+// add a new raw value to the filter, retrieve the filtered result
+float apply(float sample, float sample_last, float cutoff_freq, float dt)
+{
+    float out_put = 0.0f;
+    float rc = 0.0f;
+    float alpha = 0.0f;
+
+    if (cutoff_freq <= 0.0f || dt <= 0.0f) {
+        out_put = sample;
+        return out_put;
+    }
+
+    rc = 1.0f / (M_2PI_F*cutoff_freq);
+    alpha = constrain_float(dt / (dt + rc), 0.0f, 1.0f);
+    out_put = sample_last + (sample - sample_last) * alpha;
+    return out_put;
+}
+
+
+
+// calc_filt_alpha - recalculate the input filter alpha
+float get_filt_alpha(float _filt_hz, float _dt)
+{
+    float rc = 0.0f;
+
+    if (is_zero(_filt_hz)) {
+        return 1.0f;
+    }
+
+    // calculate alpha
+    rc = 1.0f / (2.0f* M_PI_F*_filt_hz);
+    return _dt / (_dt + rc);
+}
+
+
+// set_input_filter_d - set input to PID controller
+//  only input to the D portion of the controller is filtered
+//  this should be called before any other calls to get_p, get_i or get_d
+void set_input_filter_d(float input, float filt_hz, float dt, float *input_last, float *d_item, float *d_item_last, bool *filter_ready_flag)
+{
+    float derivative = 0.0f;
+
+    // don't process inf or NaN
+    if (!isfinite(input)) {
+        return;
+    }
+
+    // reset input filter to value received
+    if (*filter_ready_flag != true) {
+        *filter_ready_flag = true;
+        *d_item = 0.0f;
+        *d_item_last = 0.0f;
+    }
+
+    // update filter and calculate derivative
+    if (dt > 0.0f) {
+        derivative = (input - *input_last) / dt;
+        *d_item = *d_item_last + get_filt_alpha(filt_hz, dt) * (derivative - *d_item_last);
+    }
+
+    *d_item_last = *d_item;
+    *input_last = input;
+}
+
+
+// set_input_filter_all - set input to PID controller
+//  input is filtered before the PID controllers are run
+//  this should be called before any other calls to get_p, get_i or get_d
+float set_input_filter_all(float input, float filt_hz, float dt, float *input_last, float *d_item, bool *filter_ready_flag)
+{
+    float input_filt_change = 0.0f;
+
+    // don't process inf or NaN
+    if (!isfinite(input)) {
+        return 0.0f;
+    }
+
+    // reset input filter to value received
+    if (*filter_ready_flag != true) {
+        *filter_ready_flag = true;
+        *input_last = input;
+        *d_item = 0.0f;
+    }
+
+    // update filter and calculate derivative
+    input_filt_change = get_filt_alpha(filt_hz, dt) * (input - *input_last);
+    *input_last = *input_last + input_filt_change;
+
+    if (dt > 0.0f) {
+        *d_item = input_filt_change / dt;
+    }
+
+    return *input_last;
+}
+
+
+
+// set_input_filter_me - set input to PID controller
+//  only input to the D portion of the controller is filtered
+//  this should be called before any other calls to get_p, get_i or get_d
+void set_input_filter_me(float err_cur, float filt_hz, float dt, float d_item_value, float *err_last, float *d_item, float *d_item_last, bool *filter_ready_flag)
+{
+    float derivative = 0.0f;
+
+    // don't process inf or NaN
+    if (!isfinite(err_cur)) {
+        return;
+    }
+
+    // reset input filter to value received
+    if (*filter_ready_flag != true) {
+        *filter_ready_flag = true;
+        *d_item_last = 0.0f;
+    }
+
+    // update filter and calculate derivative
+    if (dt > 0.0f && d_item_value > 0.0f) {
+        derivative = (err_cur - *err_last) / dt;
+        *d_item = *d_item_last + get_filt_alpha(filt_hz, dt) * (derivative*d_item_value - *d_item_last);
+    }
+
+    *err_last = err_cur;
+    *d_item_last = *d_item;
+}

controller_yy_app/controlware/mode_attitude.c

@@ -12,10 +12,10 @@
 #include "soft_imu.h"
 #include "soft_port_uart4.h"
 #include "soft_rc_input.h"
-#include "my_math.h"
-#include "soft_motor_output.h"
+#include <my_math.h>
+#include <soft_motor_output.h>
 #include "mode_gcs_tax_launch_run.h"
-
+#include "soft_time.h"
 struct controller_xy _cxy;
 
 
@@ -319,30 +319,30 @@ static void hdw_run_no_insgps(hdw_ctx_t *ctx, float dt)
         ay_cmd = constrain_float(ctx->acc_int_y * _ATT_CXY_1_RATIO, -3.0f, 3.0f);
 
         // 姿态异常检测
-        if ((fabsf(pid_m_roll.angle_t - pid_m_roll.angle_c) > _ATT_ROLL_THRESHOLD ||
-            fabsf(pid_m_pitch.angle_t - pid_m_pitch.angle_c) > _ATT_PITCH_THRESHOLD) )
-        {
-            if(ctx->angle_monitor_triggered == false)
-            {
-                ctx->angle_monitor_triggered = true;
-                ctx->angle_over_start_t = micros();
-            }
+        // if ((fabsf(pid_m_roll.angle_t - pid_m_roll.angle_c) > _ATT_ROLL_THRESHOLD ||
+        //     fabsf(pid_m_pitch.angle_t - pid_m_pitch.angle_c) > _ATT_PITCH_THRESHOLD) )
+        // {
+        //     if(ctx->angle_monitor_triggered == false)
+        //     {
+        //         ctx->angle_monitor_triggered = true;
+        //         ctx->angle_over_start_t = micros();
+        //     }
             
-            if (micros() - ctx->angle_over_start_t > _ATT_ANGLE_PREIOD * 1e6)
-            {
-                bool roll_over  = fabsf(pid_m_roll.angle_t  - pid_m_roll.angle_c)  > _ATT_ROLL_THRESHOLD;
-                bool pitch_over = fabsf(pid_m_pitch.angle_t - pid_m_pitch.angle_c) > _ATT_PITCH_THRESHOLD;
-
-                ctx->roll_triggered  = roll_over;
-                ctx->pitch_triggered = pitch_over;
-                ctx->euler_dir_set = false;
-                angle_plan_run_cnt++;
-                no_hdw_enter_state(ctx, HDW_STATE_BACK_EULER);
-            }
-        }else
-        {
-            ctx->angle_monitor_triggered = false;
-        }
+        //     if (micros() - ctx->angle_over_start_t > _ATT_ANGLE_PREIOD * 1e6)
+        //     {
+        //         bool roll_over  = fabsf(pid_m_roll.angle_t  - pid_m_roll.angle_c)  > _ATT_ROLL_THRESHOLD;
+        //         bool pitch_over = fabsf(pid_m_pitch.angle_t - pid_m_pitch.angle_c) > _ATT_PITCH_THRESHOLD;
+
+        //         ctx->roll_triggered  = roll_over;
+        //         ctx->pitch_triggered = pitch_over;
+        //         ctx->euler_dir_set = false;
+        //         angle_plan_run_cnt++;
+        //         no_hdw_enter_state(ctx, HDW_STATE_BACK_EULER);
+        //     }
+        // }else
+        // {
+        //     ctx->angle_monitor_triggered = false;
+        // }
 
         // 周期性重置积分
         if (micros() - ctx->acc_int_start_t > _ATT_HWD_INT_PERIOD * 1e6)
@@ -429,10 +429,11 @@ void attitude_run(void)
                 // else              
                 // {
                 //    _cxy.reset = true; // 重置互定位控制器
-                    hdw_run_no_insgps(ctx, fast_loop_dt); // 无互定位状态机 
+                   // hdw_run_no_insgps(ctx, fast_loop_dt); // 无互定位状态机 
                 //}
                 
-
+                _cxy.at[0]=0;
+                _cxy.at[1]=0;
                 float dcm_z[3] = {_cxy.at[0], _cxy.at[1], GRAVITY_MSS};
                 Vector_Normalize(dcm_z, 3);
 
@@ -456,7 +457,7 @@ void attitude_run(void)
                 pid_m_roll.angle_t = apply(euler.roll * RAD_TO_DEG, t_roll_last, 15.0f, fast_loop_dt);
                 pid_m_pitch.angle_t = apply(euler.pitch * RAD_TO_DEG, t_pitch_last, 15.0f, fast_loop_dt);
 
-                no_hdw_apply_back_euler(ctx); // 打反向欧拉角时 覆盖期望欧拉
+                // no_hdw_apply_back_euler(ctx); // 打反向欧拉角时 覆盖期望欧拉
                 t_roll_last = pid_m_roll.angle_t;
                 t_pitch_last = pid_m_pitch.angle_t;
             }

controller_yy_app/controlware/my_math.c

@@ -0,0 +1,389 @@
+#include "my_math.h"
+#include "arm_math.h"
+#include <float.h>
+
+/*
+关于 DSP库的使用
+
+=================================已经测试可以使用的========================
+100个float数据的运算
+sin() 1839us-----sinf() 71us------arm_sin_f32 92us
+
+100个float数据的运算
+sqrt() 421us-----sqrtf() 39us------__sqrtf() 24us------arm_sin_f32  29us
+*/
+
+// @brief 把 buf 中数据按照小端形式搬运到 float 中
+void buf2float(float *tfloat, unsigned char *buf)
+{
+    int i;
+    unsigned char *p1 = (unsigned char *)tfloat;
+    unsigned char *p3 = buf;
+    for (i = 0; i < 4; i++)
+    {
+        *p1 = *p3;
+        p1++;
+        p3++;
+    }
+}
+
+// @brief 把 buf 中数据按照小端形式搬运到 long 中
+void buf2long(long *tfloat, unsigned char *buf)
+{
+    int i;
+    unsigned char *p1 = (unsigned char *)tfloat;
+    unsigned char *p3 = buf;
+    for (i = 0; i < 4; i++)
+    {
+        *p1 = *p3;
+        p1++;
+        p3++;
+    }
+}
+
+// @brief 把 buf 中数据按照小端形式搬运到 int 中
+void buf2int(int *tint, unsigned char *buf)
+{
+    int i;
+    unsigned char *p1 = (unsigned char *)tint;
+    unsigned char *p3 = buf;
+    for (i = 0; i < 4; i++)
+    {
+        *p1 = *p3;
+        p1++;
+        p3++;
+    }
+}
+
+// @brief 把 buf 中数据按照小端形式搬运到 short 中
+void buf2short(short *tshort, unsigned char *buf)
+{
+    int i;
+    unsigned char *p1 = (unsigned char *)tshort;
+    unsigned char *p3 = buf;
+    for (i = 0; i < 2; i++)
+    {
+        *p1 = *p3;
+        p1++;
+        p3++;
+    }
+}
+
+void float2buf(unsigned char *buf, float *tfloat)
+{
+    int i;
+    unsigned char *p1 = (unsigned char *)tfloat;
+    unsigned char *p3 = buf;
+    for (i = 0; i < 4; i++)
+    {
+        *p3 = *p1;
+        p1++;
+        p3++;
+    }
+}
+
+void int2buf(unsigned char *buf, int *tint)
+{
+    int i;
+    unsigned char *p1 = (unsigned char *)tint;
+    unsigned char *p3 = buf;
+    for (i = 0; i < 4; i++)
+    {
+        *p3 = *p1;
+        p1++;
+        p3++;
+    }
+}
+
+/*
+STM32默认是小端模式
+
+数据： 11 22 33 44
+字节： 高→→→→→→→低
+
+小端模式
+地址：0   1   2   3
+内存：44  33  22  11
+
+大端模式
+地址：0   1   2   3
+内存：11  22  33  44
+*/
+void short2buf(unsigned char *buf, short *tshort)
+{
+    int i;
+    unsigned char *p1 = (unsigned char *)tshort;
+    unsigned char *p3 = buf;
+    for (i = 0; i < 2; i++)
+    {
+        *p3 = *p1;
+        p1++;
+        p3++;
+    }
+}
+
+int brinv(float a[], int n)
+{
+    int *is, *js, i, j, k, l, u, v;
+    int temp1[5];
+    int temp2[5];
+    float d, p;
+    is = temp1;
+    js = temp2;
+    for (k = 0; k <= n - 1; k++)
+    {
+        d = 0.0f;
+        for (i = k; i <= n - 1; i++)
+            for (j = k; j <= n - 1; j++)
+            {
+                l = i * n + j;
+                p = fabsf(a[l]);
+                if (p > d)
+                {
+                    d = p;
+                    is[k] = i;
+                    js[k] = j;
+                }
+            }
+
+        if (d + 1.0f == 1.0f)
+        {
+            return (0);
+        }
+        if (is[k] != k)
+        {
+            for (j = 0; j <= n - 1; j++)
+            {
+                u = k * n + j;
+                v = is[k] * n + j;
+                p = a[u];
+                a[u] = a[v];
+                a[v] = p;
+            }
+        }
+        if (js[k] != k)
+            for (i = 0; i <= n - 1; i++)
+            {
+                u = i * n + k;
+                v = i * n + js[k];
+                p = a[u];
+                a[u] = a[v];
+                a[v] = p;
+            }
+        l = k * n + k;
+        a[l] = 1.0f / a[l];
+        for (j = 0; j <= n - 1; j++)
+            if (j != k)
+            {
+                u = k * n + j;
+                a[u] = a[u] * a[l];
+            }
+        for (i = 0; i <= n - 1; i++)
+            if (i != k)
+                for (j = 0; j <= n - 1; j++)
+                    if (j != k)
+                    {
+                        u = i * n + j;
+                        a[u] = a[u] - a[i * n + k] * a[k * n + j];
+                    }
+        for (i = 0; i <= n - 1; i++)
+            if (i != k)
+            {
+                u = i * n + k;
+                a[u] = -a[u] * a[l];
+            }
+    }
+    for (k = n - 1; k >= 0; k--)
+    {
+        if (js[k] != k)
+            for (j = 0; j <= n - 1; j++)
+            {
+                u = k * n + j;
+                v = js[k] * n + j;
+                p = a[u];
+                a[u] = a[v];
+                a[v] = p;
+            }
+        if (is[k] != k)
+            for (i = 0; i <= n - 1; i++)
+            {
+                u = i * n + k;
+                v = i * n + is[k];
+                p = a[u];
+                a[u] = a[v];
+                a[v] = p;
+            }
+    }
+    return (1);
+}
+
+unsigned char constrain_uint8(unsigned char value, unsigned char min,
+                              unsigned char max)
+{
+    if (value > max)
+        value = max;
+    else if (value < min)
+        value = min;
+    return value;
+}
+
+short math_constrain(short value, short min, short max)
+{
+    if (value > max)
+        value = max;
+    else if (value < min)
+        value = min;
+    return value;
+}
+
+// constrain a value
+// float constrain_float(float amt, float low, float high)
+// {
+//     // the check for NaN as a float prevents propogation of
+//     // floating point errors through any function that uses
+//     // constrain_float(). The normal float semantics already handle -Inf
+//     // and +Inf
+//     if (isnan(amt))
+//     {
+//         return (low + high) * 0.5f;
+//     }
+//     return ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)));
+// }
+
+// constrain a int16_t value
+int16_t constrain_int16(int16_t amt, int16_t low, int16_t high)
+{
+    return ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)));
+}
+
+// constrain a int32_t value
+int32_t constrain_int32(int32_t amt, int32_t low, int32_t high)
+{
+    return ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)));
+}
+
+short abs_int16(short value)
+{
+    if ((value > 0) || (value == 0))
+        return value;
+    return -value;
+}
+
+float min_float(float value1, float value2)
+{
+    if (value1 < value2)
+        return value1;
+    return value2;
+}
+
+int min_int32(int value1, int value2)
+{
+    if (value1 < value2)
+        return value1;
+    return value2;
+}
+
+short min_int16(short value1, short value2)
+{
+    if (value1 < value2)
+        return value1;
+    return value2;
+}
+
+float max_float(float value1, float value2)
+{
+    if (value1 > value2)
+        return value1;
+    return value2;
+}
+
+int max_int32(int value1, int value2)
+{
+    if (value1 > value2)
+        return value1;
+    return value2;
+}
+
+short max_int16(short value1, short value2)
+{
+    if (value1 > value2)
+        return value1;
+    return value2;
+}
+
+// is a float is zero
+bool is_zero(const float fVal1)
+{
+    return fabsf(fVal1) < FLT_EPSILON ? true : false;
+}
+
+float no_zero_float(float Val1)
+{
+    if (Val1 == 0.0f)
+        return 0.1f;
+    return Val1;
+}
+
+int no_zero_int32(int Val1)
+{
+    if (Val1 == 0)
+        return 1;
+    return Val1;
+}
+
+// square
+float sq(float v) { return v * v; }
+
+// @brief 获取两个数的平方和开根号
+float get_norm(float a, float b)
+{
+    float norm;
+    arm_sqrt_f32(sq(a) + sq(b), &norm);
+
+    return norm;
+}
+
+// 计算数据的标准差
+// float stdev(short num[], int n)
+// {
+//     int i = 0;
+//     float sum = 0.0f;
+//     float avg = 0.0f;
+//
+//     sum = 0.0f;
+//     for (i = 0; i < n; i++)
+//     {
+//         sum += num[i];
+//     }
+//
+//     avg = sum / n;
+//
+//     sum = 0.0f;
+//     for (i = 0; i < n; i++)
+//     {
+//         sum += (num[i] - avg) * (num[i] - avg);
+//     }
+//
+//     return (__sqrtf(sum / n));
+// }
+
+/** @brief 插入排序算法
+ * @param int arr[]被排数列，int n 排序窗宽
+ * @retval none
+ */
+void insert_sort(int arr[], int n)
+{
+    int temp;
+    int i, j;
+    for (i = 1; i < n; ++i)
+    {
+        temp = arr[i];
+        for (j = i; j > 0 && temp < arr[j - 1]; --j)
+        {
+            arr[j] = arr[j - 1];
+        }
+        arr[j] = temp;
+    }
+}
+
+// =========================

controller_yy_app/controlware/pilot_init.c

@@ -0,0 +1,106 @@
+#include "pilot_init.h"
+#include "auto_pilot.h"
+#include "bsp_V8M_YY_led.h"
+#include "bsp_V8M_adc.h"
+#include "bsp_V8M_flash.h"
+#include "data_save.h"
+#include "drv_usart.h"
+#include "flight_mode.h"
+#include "params.h"
+#include "soft_can.h"
+#include "soft_delay.h"
+#include "soft_flash.h"
+#include "soft_gps.h"
+#include "soft_gs.h"
+#include "soft_imu.h"
+#include "soft_motor_output.h"
+#include "soft_port_uart4.h"
+#include "soft_rc_input.h"
+#include "soft_sdcard.h"
+#include "soft_time.h"
+#include "soft_timer.h"
+#include "soft_voltage.h"
+#include "stdio.h"
+#include "stm32f4xx.h"
+#include "ver_config.h"
+
+#define GCS_UART_Baudrate 115200 // 与地面站通信波特率
+#define UART3_Baudrate 460800    // 与IMU通信波特率
+#define UART4_Baudrate 115200    // 外接设备通信波特率
+
+void auto_pilot_init(void)
+{
+    /*****************************************************系统初始化**************************************************/
+    // 配置系统时钟为168M 使用外部8M晶体+PLL
+    SystemInit();
+    // 中断向量表偏移
+    SCB->VTOR = FLASH_BASE | 0x10000;
+    // 设置中断向量组，1抢占优先级，3响应优先级
+    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
+    // 延时初始化,并启动开机时间。
+    system_delay_initial(168);
+    // 启动系统主定时器 1us 总计数时间为0xffffffff * 0.000001
+    system_time_initial();
+    // 系统定时器中断初始化
+    system_timer_initial();
+
+    // 初始化 SPI总线
+    flash_at45db_initial();
+    // 检查flash是否在电路班上
+    while (!flash_isexist())
+    {
+        // 查询AT45DB 是否在电路板上
+        delay_ms(50);
+    }
+    // 初始化 四轴姿态PID，初始化参数放在前面，因为里面包含有遥控器的校准系数。
+    // 不然一直到获取到校准系数之前，遥控器的输入都是停留在1500us，导致无法进入电调校准的识别
+    initial_parameters();
+
+    // SBUS输入
+    rc_input_initial();
+
+    // 初始化 can 总线接口
+    CAN_BusInit();
+
+    // 电压接口初始化
+    Voltage_Init();
+
+    // PWM输出
+    motor_output_initial();
+
+    // 等待接收机器件上电
+    delay_ms(200);
+
+    // 带油门 上电，说明用户希望初始化电调
+    esc_calibrate_enable();
+
+    if (ver_par.hardware_id == HW_V8M_YY)
+    {
+        v8m_yy_led_init();
+    }
+
+    // 串口3
+    imu_uart3_initial(UART3_Baudrate);
+
+    // 地面站初始化
+    gcs_init(&gcs_link, GCS_UART_Baudrate);
+
+    // 检查升级标志位
+    switch (ver_par.hardware_id)
+    {
+    case HW_V8M_YY:
+        V8M_check_iap_flag();
+        break;
+
+    default:
+        break;
+    }
+    
+    // 初始化SDCARD
+    sdcard_inital(0);
+    // 读取最后一条log信息，初始化log结构体
+    sd_card_init_fly_log();
+
+    // 串口4
+    port_uart4_initial();
+}

controller_yy_app/controlware/pilot_navigation.c

@@ -0,0 +1,241 @@
+#include "pilot_navigation.h"
+#include "arm_math.h"
+#include "auto_pilot.h"
+#include "common.h"
+#include "control_attitude.h"
+#include "control_throttle.h"
+#include "flight_mode.h"
+#include "geomatry.h"
+#include "helpler_funtions.h"
+#include "lowpass_filter.h"
+#include "my_math.h"
+#include "params.h"
+#include "soft_flash.h"
+#include "soft_gs.h"
+#include "soft_imu.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 <stdlib.h>
+
+struct rec_pos home_position;
+struct rec_pos takeoff_position;
+struct rec_pos poshold_position;
+struct rec_pos wpphoto_position;
+struct rec_pos circlecenter_position;
+struct rec_pos target_yaw_lock_position;
+
+// 航线圈数
+uint16_t wp_cycle_times = 1;
+uint16_t wp_have_cycle_times = 0;
+
+// 机头到达位置的时间
+uint32_t wp_reach_yaw_time;
+
+void record_position(int *r_lng, int *r_lat, int lng, int lat)
+{
+    *r_lng = lng;
+    *r_lat = lat;
+}
+
+// @brief 根究纬度计算余弦因子
+float earth_longitude_scale(int lat)
+{
+    float earth_lngscale = cosf(fabsf(
+        (lat / 10000000.0f) * DEG_TO_RAD)); // 1角度 = PI/180 = 0.0174532925弧度
+
+    return earth_lngscale;
+}
+
+// @brief 计算 wgs48 经纬度两点的距离
+// @note 使用纬度余弦因子近似弥补误差
+int point_to_point_distance(int lon1, int lat1, int lon2, int lat2)
+{
+    float dist = 0;
+    double cmLat = 0.0f;
+    double cmLon = 0.0f;
+
+    cmLat = wrap_double((double)(lat2) - (double)(lat1), -180 * 1e7, 180 * 1e7);
+    cmLon = wrap_double((double)(lon2) - (double)(lon1), -180 * 1e7, 180 * 1e7) * earth_longitude_scale(lat2);
+
+    arm_sqrt_f32(sq(cmLat) + sq(cmLon), &dist);
+    dist *= LONLAT_TO_CM;
+
+    return dist;
+}
+
+
+
+
+
+float cal_tar_vel_z(int t_alt, int c_alt, int acc_up, int acc_down)
+{
+    float tar_vel = pid_v_alt.dist_p * (t_alt - c_alt);
+
+    tar_vel = constrain_float(
+        tar_vel, -1.0f * parinf._par_max_approach_rate_automode * 10.0f,
+        parinf._par_max_climb_rate_automode * 10.0f);
+
+    float delt_vel_up_max = acc_up / 200.0f;
+    float delt_vel_down_max = acc_down / 200.0f;
+
+    if (althold_state == NO_ALT_HOLD)
+    {
+        // 如果飞机低于目标高度 1m 外，则慢加快减
+        if (t_alt - c_alt > 100)
+        {
+            if (tar_vel - pid_m_alt.vel_t > 1.0f)
+            {
+                tar_vel = pid_m_alt.vel_t + 1.0f;
+            }
+            else if (tar_vel - pid_m_alt.vel_t < -delt_vel_down_max)
+            {
+                tar_vel = pid_m_alt.vel_t - delt_vel_down_max;
+            }
+        }
+        // 如果飞机高于目标高度 1m 外，则快加慢减
+        else if (t_alt - c_alt < -100)
+        {
+            if (tar_vel - pid_m_alt.vel_t > delt_vel_up_max)
+            {
+                tar_vel = pid_m_alt.vel_t + delt_vel_up_max;
+            }
+            else if (tar_vel - pid_m_alt.vel_t < -1.0f)
+            {
+                tar_vel = pid_m_alt.vel_t - 1.0f;
+            }
+        }
+        // 如果飞机高度在目标高度 1m 内，则按照正常的逻辑计算目标垂直速度
+
+        // 如果距离目标高度在 10 m 内，则注意速度不要超过 0.6 * 高度误差
+        if (abs(t_alt - c_alt) < 1000)
+        {
+            float tmp_velz = fabsf((t_alt - c_alt) * 0.6f);
+
+            float min_target_velz = (tmp_velz < 100 ? 100 : tmp_velz);
+
+            if (fabsf(tar_vel) > min_target_velz)
+            {
+                tar_vel = min_target_velz * tar_vel / fabsf(tar_vel);
+            }
+        }
+    }
+
+    return tar_vel;
+}
+
+
+bool reset_wp_start_time_flag = false;
+uint32_t start_vel_time = 0, time_period = 0;
+
+#define WP_ACC 100.0f
+/**
+ * @brief 根据时间计算起停速度
+ */
+int cal_tar_vel_xy_unac(int h_dist, int r_dist, int min_vel, int max_vel)
+{
+    // 测试方法三：按照固定加速度执行。
+    float start_vel, stop_vel;
+    static int init_vel = 0.0f;
+
+    if (r_dist < 0)
+        r_dist = 0;
+
+    if (max_vel < 50)
+        max_vel = 50;
+
+    if (reset_wp_start_time_flag == true)
+    {
+        start_vel_time = micros();
+        time_period = 0;
+        init_vel = ins.horz_vel;
+        reset_wp_start_time_flag = false;
+    }
+
+    time_period += micros() - start_vel_time;
+    start_vel_time = micros();
+
+    start_vel = init_vel + WP_ACC * time_period / 1000000.0f;
+    start_vel = constrain_int32(start_vel, 0, max_vel);
+
+    if (r_dist > 500)
+    {
+        arm_sqrt_f32(2 * 0.8f * WP_ACC * (r_dist - 500), &stop_vel);
+        stop_vel = constrain_int32(stop_vel, min_vel, max_vel);
+    }
+    else
+        stop_vel = min_vel;
+
+    return min_int32(start_vel, stop_vel);
+}
+
+// 偏航距的PID，输出一个速度的角度差
+float drift_crosstrack_p = 0.0f, 
+      drift_crosstrack_d = 0.0f, drift_crosstrack_d_last = 0.0f;
+
+
+
+float drift_crosstrack_vel_integ = 0.0f;
+
+// 航点总数
+unsigned short waypoint_totalnum = 0;
+
+// 目标航点序号
+unsigned short tar_wp_no = 0;
+
+
+// 前一个点到目标点的距离 cm
+int wp_prevtotar_distance = 0;
+// 前一个点到目标点的方位角 deg
+float wp_prevtotar_bearing = 0.0f;
+
+// 前一个点到当前点的距离 cm
+int wp_prevtocur_distance = 0;
+// 前一个点到当前点的方位角 deg
+float wp_prevtocur_bearing = 0.0f;
+
+// 当前点到目标点的的距离 cm
+int wp_curtotar_distance = 0;
+// 当前点到目标点的的方位角 deg
+float wp_curtotar_bearing = 0.0f;
+
+// 开始点到当前点的垂线距离
+int wp_prevtocur_verdistance = 0;
+// 当前点到目标点的垂线距离，距离有正负
+int wp_curtotar_verdistance = 0;
+
+// 当前点到下条航线的垂直距离
+int wp_cur_to_next_wpline_verdistance = 0;
+
+bool fly_point_flag = false, update_point_flag = false,
+     execute_command_flag = false;
+
+
+/**************************实现函数********************************************
+ *函数原型:		void update_nav_point(void);
+ *功　　能:	    更新航点的信息
+ *******************************************************************************/
+
+// 到达目标航点高度标志位
+bool tarpoint_alt_isarrive = false;
+// 到达目标航点位置标志位
+bool tarpoint_pos_isarrive = false;
+
+// U 型转弯航向转动标志
+bool coordinatemode_yaw_turn = false;
+
+
+float wp_curtotar_bearing_last = 0.0f;
+float wp_prevtotar_bearing_last = 0.0f;
+
+unsigned int arrive_point_time = 0;
+
+
+
+// 航线目标水平速度
+static int desire_vel_xy = 0; // CM/S
+// 航线目标垂直速度
+static int desire_vel_z = 0; // CM/S
+

controller_yy_app/hardware/hard_can.c

@@ -80,29 +80,29 @@ static volatile  bool has_sent_out;
 static volatile  bool has_error;
 static volatile  can_receive_buf_t s_can_rx_buf;
 static volatile  uint8_t error_flags;
-
-SDK_DECLARE_EXT_ISR_M(CAN2_IRQ, can2_isr)
-void can2_isr(void)
-{
-    uint8_t flags = can_get_tx_rx_flags(CAN2);
-    if ((flags & CAN_EVENT_RECEIVE) != 0) {
-        can_read_received_message(CAN2, (can_receive_buf_t *)&s_can_rx_buf);
-        has_new_rcv_msg = true;
-    }
-    if ((flags & (CAN_EVENT_TX_PRIMARY_BUF | CAN_EVENT_TX_SECONDARY_BUF))) {
-        has_sent_out = true;
-    }
-    if ((flags & CAN_EVENT_ERROR) != 0) {
-        has_error = true;
-    }
-    can_clear_tx_rx_flags(CAN2, flags);
-
-    error_flags = can_get_error_interrupt_flags(CAN2);
-    if (error_flags != 0) {
-        has_error = true;
-    }
-    can_clear_error_interrupt_flags(CAN2, error_flags);
-}
+// 中断函数在soft can 那里
+//SDK_DECLARE_EXT_ISR_M(CAN2_IRQ, can2_isr)
+//void can2_isr(void)
+//{
+//    uint8_t flags = can_get_tx_rx_flags(CAN2);
+//    if ((flags & CAN_EVENT_RECEIVE) != 0) {
+//        can_read_received_message(CAN2, (can_receive_buf_t *)&s_can_rx_buf);
+//        has_new_rcv_msg = true;
+//    }
+//    if ((flags & (CAN_EVENT_TX_PRIMARY_BUF | CAN_EVENT_TX_SECONDARY_BUF))) {
+//        has_sent_out = true;
+//    }
+//    if ((flags & CAN_EVENT_ERROR) != 0) {
+//        has_error = true;
+//    }
+//    can_clear_tx_rx_flags(CAN2, flags);
+
+//    error_flags = can_get_error_interrupt_flags(CAN2);
+//    if (error_flags != 0) {
+//        has_error = true;
+//    }
+//    can_clear_error_interrupt_flags(CAN2, error_flags);
+//}
 
 static uint8_t can_get_data_bytes_from_dlc(uint32_t dlc)
 {

controller_yy_app/hardware/hard_inc/hard_rc_sbus.h

@@ -8,7 +8,7 @@
 
 //这个值要设置的比25大一些。正好是25的话获取的DMA_GetCurrDataCounter(DMA1_Stream5)会是25.
 #define UART2_RX_LENDTH_MAX 30
-extern unsigned char sbus_rx_buf[UART2_RX_LENDTH_MAX];
+extern unsigned char uart2_dma_rx_buf[UART2_RX_LENDTH_MAX]; // 实际是串口中断接收 不是dma
 
 extern void rc_sbus_init(uint32_t baud);
 // 由于hpm6750没有串口空闲中断， 所以采用串口超时中断的而形式 FIFO深度16字节

controller_yy_app/hardware/hard_rc_subs.c

@@ -13,7 +13,7 @@
 #define SBUS_UART2_IRQ_RANK            2
 
 /* 接收缓冲区 */
-ATTR_PLACE_AT_NONCACHEABLE uint8_t sbus_rx_buf[UART2_RX_LENDTH_MAX];
+ATTR_PLACE_AT_NONCACHEABLE uint8_t uart2_dma_rx_buf[UART2_RX_LENDTH_MAX];
 
 /* 全局变量 */
 static volatile uint16_t sbus_rx_index = 0;
@@ -40,7 +40,7 @@ void uart2_sbus_isr(void)
         /* 从FIFO中读取数据 */
         while (uart_check_status(SBUS_UART2_RX, uart_stat_data_ready)) {
             count++;
-            sbus_rx_buf[sbus_rx_index++] = uart_read_byte(SBUS_UART2_RX);
+            uart2_dma_rx_buf[sbus_rx_index++] = uart_read_byte(SBUS_UART2_RX);
             
             /* 防止缓冲区溢出 */
             if (sbus_rx_index >= UART2_RX_LENDTH_MAX) {
@@ -60,9 +60,10 @@ void uart2_sbus_isr(void)
         
         /* 读取FIFO中剩余的所有数据 */
         while (uart_check_status(SBUS_UART2_RX, uart_stat_data_ready)) {
-            sbus_rx_buf[sbus_rx_index++] = uart_read_byte(SBUS_UART2_RX);
+            uart2_dma_rx_buf[sbus_rx_index++] = uart_read_byte(SBUS_UART2_RX);
             // 回调解析数据   
         }
+        recv_rcsbus_data_hookfunction(sbus_rx_index, uart2_dma_rx_buf);
         // recv_rcsbus_data_hookfunction(sbus_rx_index, sbus_rx_buf);
         /* 标记一帧SBUS数据接收完成 */
 
@@ -147,7 +148,7 @@ void uart2_sbus_test(void)
 
         while(sbus_frame_length--)
         {
-            printf(" %c ",sbus_rx_buf[sbus_frame_length]);
+            printf(" %c ",uart2_dma_rx_buf[sbus_frame_length]);
         }
       }
     

controller_yy_app/hardware/hard_system.c

@@ -24,7 +24,7 @@ void sys_reset(void)
 void u3_dma_disable(void)
 {
     // 禁用HDMA通道 1
-    dma_disable_channel(U3_DMA, U3_DMA_CH);
+    dma_disable_channel(U3_DMA, U3_DMA_CH); 
     
 }
 

controller_yy_app/middleware/hpm_sdmmc/port/hpm_sdmmc_port.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021-2024 HPMicro
+ * Copyright (c) 2021-2023 HPMicro
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -7,11 +7,10 @@
 #include "hpm_common.h"
 #include "hpm_soc.h"
 #include "hpm_sdmmc_host.h"
-// #include "board.h"
-#include "hard_sdio_sd.h"
-#include "hard_system_delay.h"
+#include "board.h"
 
-uint32_t sdmmc_get_sys_addr(const sdmmc_host_t *host, uint32_t addr)
+
+uint32_t sdmmc_get_sys_addr(sdmmc_host_t *host, uint32_t addr)
 {
     return core_local_mem_to_sys_address(host->host_param.hart_id, addr);
 }
@@ -24,15 +23,15 @@ ATTR_WEAK hpm_stat_t board_init_sd_host_params(sdmmc_host_t *host, SDMMCHOST_Typ
     sdmmc_io_init_apis_t *init_apis = &host->host_param.io_init_apis;
     hpm_sdmmc_extra_io_data_t *io_data = &host->host_param.io_data;
     param->base = base;
-    param->clock_init_func = sd1_configure_clock;
-    param->hart_id = RUN_CORE;
-    param->delay_ms = system_delay_ms;
+    param->clock_init_func = board_sd_configure_clock;
+    param->hart_id = BOARD_RUNNING_CORE;
+    param->delay_ms = board_delay_ms;
 
-    init_apis->cd_io_init = sdxc1_cd_pin;
-    init_apis->cmd_io_init = sdxc1_cmd_pin;
-    init_apis->clk_data_io_init = sdxc1_clk_data_pins;
+    init_apis->cd_io_init = init_sdxc_cd_pin;
+    init_apis->cmd_io_init = init_sdxc_cmd_pin;
+    init_apis->clk_data_io_init = init_sdxc_clk_data_pins;
 
-#if defined(SDCARD_SUPPORT_3V3) && (SDCARD_SUPPORT_3V3 == 1)
+#if defined(BOARD_APP_SDCARD_SUPPORT_3V3) && (BOARD_APP_SDCARD_SUPPORT_3V3 == 1)
     param->host_flags |= HPM_SDMMC_HOST_SUPPORT_3V3;
 #endif
     bool support_1v8 = false;
@@ -40,11 +39,11 @@ ATTR_WEAK hpm_stat_t board_init_sd_host_params(sdmmc_host_t *host, SDMMCHOST_Typ
     bool support_vsel = false;
     bool support_pwr = false;
     bool support_cd = false;
-#if defined(SDCARD_SUPPORT_1V8) && (SDCARD_SUPPORT_1V8 == 1)
+#if defined(BOARD_APP_SDCARD_SUPPORT_1V8) && (BOARD_APP_SDCARD_SUPPORT_1V8 == 1)
     param->host_flags |= HPM_SDMMC_HOST_SUPPORT_1V8;
     support_1v8 = true;
 #endif
-#if defined(SDCARD_SUPPORT_4BIT) && (SDCARD_SUPPORT_4BIT == 1)
+#if defined(BOARD_APP_SDCARD_SUPPORT_4BIT) && (BOARD_APP_SDCARD_SUPPORT_4BIT == 1)
     param->host_flags |= HPM_SDMMC_HOST_SUPPORT_4BIT;
     support_4bit = true;
 #endif
@@ -55,29 +54,29 @@ ATTR_WEAK hpm_stat_t board_init_sd_host_params(sdmmc_host_t *host, SDMMCHOST_Typ
         }
     }
 
-#if defined(SDCARD_SUPPORT_CARD_DETECTION) && (SDCARD_SUPPORT_CARD_DETECTION == 1)
+#if defined(BOARD_APP_SDCARD_SUPPORT_CARD_DETECTION) && (BOARD_APP_SDCARD_SUPPORT_CARD_DETECTION == 1)
     param->host_flags |= HPM_SDMMC_HOST_SUPPORT_CARD_DETECTION;
     support_cd = true;
-    init_apis->cd_io_init = sdxc1_cd_pin;
+    init_apis->cd_io_init = init_sdxc_cd_pin;
 #endif
 
-#if defined(SDCARD_SUPPORT_POWER_SWITCH) && (SDCARD_SUPPORT_POWER_SWITCH == 1)
+#if defined(BOARD_APP_SDCARD_SUPPORT_POWER_SWITCH) && (BOARD_APP_SDCARD_SUPPORT_POWER_SWITCH == 1)
     param->host_flags |= HPM_SDMMC_HOST_SUPPORT_POWER_SWITCH;
     support_pwr = true;
     init_apis->pwr_io_init = init_sdxc_pwr_pin;
 #endif
 
-#if defined(SDCARD_SUPPORT_VOLTAGE_SWITCH) && (SDCARD_SUPPORT_VOLTAGE_SWITCH == 1)
+#if defined(BOARD_APP_SDCARD_SUPPORT_VOLTAGE_SWITCH) && (BOARD_APP_SDCARD_SUPPORT_VOLTAGE_SWITCH == 1)
     param->host_flags |= HPM_SDMMC_HOST_SUPPORT_VOLTAGE_SWITCH;
     support_vsel = true;
-    init_apis->vsel_io_init = sdxc1_vsel_pin;
+    init_apis->vsel_io_init = init_sdxc_vsel_pin;
 #endif
 
     if (support_vsel) {
-#if defined(SDCARD_VOLTAGE_SWITCH_USING_GPIO) && (BSDCARD_VOLTAGE_SWITCH_USING_GPIO == 1)
+#if defined(BOARD_APP_SDCARD_VOLTAGE_SWITCH_USING_GPIO) && (BOARD_APP_SDCARD_VOLTAGE_SWITCH_USING_GPIO == 1)
         io_data->vsel_pin.use_gpio = true;
-        io_data->vsel_pin.gpio_pin = SDCARD_VSEL_PIN;
-#if defined(SDCARD_VOLTAGE_SWITCH_PIN_POL) && (SDCARD_VOLTAGE_SWITCH_PIN_POL == 1)
+        io_data->vsel_pin.gpio_pin = BOARD_APP_SDCARD_VSEL_PIN;
+#if defined(BOARD_APP_SDCARD_VOLTAGE_SWITCH_PIN_POL) && (BOARD_APP_SDCARD_VOLTAGE_SWITCH_PIN_POL == 1)
         io_data->vsel_pin.polarity = 1;
 #else
         io_data->vsel_pin.polarity = 0;
@@ -88,10 +87,10 @@ ATTR_WEAK hpm_stat_t board_init_sd_host_params(sdmmc_host_t *host, SDMMCHOST_Typ
 #endif
     }
     if (support_cd) {
-#if defined(SDCARD_CARD_DETECTION_USING_GPIO) && (SDCARD_CARD_DETECTION_USING_GPIO == 1)
+#if defined(BOARD_APP_SDCARD_CARD_DETECTION_USING_GPIO) && (BOARD_APP_SDCARD_CARD_DETECTION_USING_GPIO == 1)
         io_data->cd_pin.use_gpio = true;
-        io_data->cd_pin.gpio_pin = SDCARD_CARD_DETECTION_PIN;
-#if defined(SDCARD_CARD_DETECTION_PIN_POL) && (SDCARD_CARD_DETECTION_PIN_POL == 1)
+        io_data->cd_pin.gpio_pin = BOARD_APP_SDCARD_CARD_DETECTION_PIN;
+#if defined(BOARD_APP_SDCARD_CARD_DETECTION_PIN_POL) && (BOARD_APP_SDCARD_CARD_DETECTION_PIN_POL == 1)
         io_data->cd_pin.polarity = 1;
 #else
         io_data->cd_pin.polarity = 0;
@@ -103,10 +102,10 @@ ATTR_WEAK hpm_stat_t board_init_sd_host_params(sdmmc_host_t *host, SDMMCHOST_Typ
     }
 
     if (support_pwr) {
-#if defined(SDCARD_POWER_SWITCH_USING_GPIO) && (SDCARD_POWER_SWITCH_USING_GPIO == 1)
+#if defined(BOARD_APP_SDCARD_POWER_SWITCH_USING_GPIO) && (BOARD_APP_SDCARD_POWER_SWITCH_USING_GPIO == 1)
         io_data->pwr_pin.use_gpio = true;
-        io_data->pwr_pin.gpio_pin = SDCARD_POWER_SWITCH_PIN;
-#if defined(SDCARD_POWER_SWITCH_PIN_POL) && (SDCARD_POWER_SWITCH_PIN_POL == 1)
+        io_data->pwr_pin.gpio_pin = BOARD_APP_SDCARD_POWER_SWITCH_PIN;
+#if defined(BOARD_APP_SDCARD_POWER_SWITCH_PIN_POL) && (BOARD_APP_SDCARD_POWER_SWITCH_PIN_POL == 1)
         io_data->pwr_pin.polarity = true;
 #else
         io_data->pwr_pin.polarity = false;
@@ -128,20 +127,20 @@ ATTR_WEAK hpm_stat_t board_init_emmc_host_params(sdmmc_host_t *host, SDMMCHOST_T
 
     sdmmc_io_init_apis_t *init_apis = &host->host_param.io_init_apis;
     hpm_sdmmc_extra_io_data_t *io_data = &host->host_param.io_data;
-    param->base = EMMC_SDXC_BASE;
-    param->clock_init_func = sd1_configure_clock;
-    param->hart_id = RUN_CORE;
-    param->delay_ms = system_delay_ms;
-
-    init_apis->cd_io_init = sdxc1_cd_pin;
-    init_apis->cmd_io_init = sdxc1_cmd_pin;
-    init_apis->clk_data_io_init = sdxc1_clk_data_pins;
-#if defined(EMMC_SUPPORT_DS) && (EMMC_SUPPORT_DS == 1)
+    param->base = BOARD_APP_EMMC_SDXC_BASE;
+    param->clock_init_func = board_sd_configure_clock;
+    param->hart_id = BOARD_RUNNING_CORE;
+    param->delay_ms = board_delay_ms;
+
+    init_apis->cd_io_init = init_sdxc_cd_pin;
+    init_apis->cmd_io_init = init_sdxc_cmd_pin;
+    init_apis->clk_data_io_init = init_sdxc_clk_data_pins;
+#if defined(BOARD_APP_EMMC_SUPPORT_DS) && (BOARD_APP_EMMC_SUPPORT_DS == 1)
     init_apis->ds_io_init = init_sdxc_ds_pin;
     support_ds = true;
 #endif
 
-#if defined(EMMC_SUPPORT_3V3) && (EMMC_SUPPORT_3V3 == 1)
+#if defined(BOARD_APP_EMMC_SUPPORT_3V3) && (BOARD_APP_EMMC_SUPPORT_3V3 == 1)
     param->host_flags |= HPM_SDMMC_HOST_SUPPORT_3V3;
 #endif
     bool support_1v8 = false;
@@ -149,15 +148,15 @@ ATTR_WEAK hpm_stat_t board_init_emmc_host_params(sdmmc_host_t *host, SDMMCHOST_T
     bool support_8bit = false;
     bool support_vsel = false;
     bool support_pwr = false;
-#if defined(EMMC_SUPPORT_1V8) && (EMMC_SUPPORT_1V8 == 1)
+#if defined(BOARD_APP_EMMC_SUPPORT_1V8) && (BOARD_APP_EMMC_SUPPORT_1V8 == 1)
     param->host_flags |= HPM_SDMMC_HOST_SUPPORT_1V8;
     support_1v8 = true;
 #endif
-#if defined(EMMC_SUPPORT_4BIT) && (EMMC_SUPPORT_4BIT == 1)
+#if defined(BOARD_APP_EMMC_SUPPORT_4BIT) && (BOARD_APP_EMMC_SUPPORT_4BIT == 1)
     param->host_flags |= HPM_SDMMC_HOST_SUPPORT_4BIT;
     support_4bit = true;
 #endif
-#if defined(EMMC_SUPPORT_8BIT) && (EMMC_SUPPORT_8BIT == 1)
+#if defined(BOARD_APP_EMMC_SUPPORT_8BIT) && (BOARD_APP_EMMC_SUPPORT_8BIT == 1)
     param->host_flags |= HPM_SDMMC_HOST_SUPPORT_4BIT | HPM_SDMMC_HOST_SUPPORT_8BIT;
     support_8bit = true;
     support_4bit = true;
@@ -172,27 +171,27 @@ ATTR_WEAK hpm_stat_t board_init_emmc_host_params(sdmmc_host_t *host, SDMMCHOST_T
         param->host_flags |= HPM_SDMMC_HOST_SUPPORT_DDR;
     }
 
-#if defined(EMMC_SUPPORT_DS) && (EMMC_SUPPORT_DS == 1)
+#if defined(BOARD_APP_EMMC_SUPPORT_DS) && (BOARD_APP_EMMC_SUPPORT_DS == 1)
     param->host_flags |= HPM_SDMMC_HOST_SUPPORT_DATA_STROBE;
 #endif
 
-#if defined(EMMC_SUPPORT_POWER_SWITCH) && (EMMC_SUPPORT_POWER_SWITCH == 1)
+#if defined(BOARD_APP_EMMC_SUPPORT_POWER_SWITCH) && (BOARD_APP_EMMC_SUPPORT_POWER_SWITCH == 1)
     param->host_flags |= HPM_SDMMC_HOST_SUPPORT_POWER_SWITCH;
     support_pwr = true;
     init_apis->pwr_io_init = init_sdxc_pwr_pin;
 #endif
 
-#if defined(EMMC_SUPPORT_VOLTAGE_SWITCH) && (EMMC_SUPPORT_VOLTAGE_SWITCH == 1)
+#if defined(BOARD_APP_EMMC_SUPPORT_VOLTAGE_SWITCH) && (BOARD_APP_EMMC_SUPPORT_VOLTAGE_SWITCH == 1)
     param->host_flags |= HPM_SDMMC_HOST_SUPPORT_VOLTAGE_SWITCH;
     support_vsel = true;
     init_apis->vsel_io_init = init_sdxc_vsel_pin;
 #endif
 
     if (support_vsel) {
-#if defined(EMMC_VOLTAGE_SWITCH_USING_GPIO) && (EMMC_VOLTAGE_SWITCH_USING_GPIO == 1)
+#if defined(BOARD_APP_EMMC_VOLTAGE_SWITCH_USING_GPIO) && (BOARD_APP_EMMC_VOLTAGE_SWITCH_USING_GPIO == 1)
         io_data->vsel_pin.use_gpio = true;
-        io_data->vsel_pin.gpio_pin = EMMC_VSEL_PIN;
-#if defined(EMMC_VOLTAGE_SWITCH_PIN_POL) && (EMMC_VOLTAGE_SWITCH_PIN_POL == 1)
+        io_data->vsel_pin.gpio_pin = BOARD_APP_EMMC_VSEL_PIN;
+#if defined(BOARD_APP_EMMC_VOLTAGE_SWITCH_PIN_POL) && (BOARD_APP_EMMC_VOLTAGE_SWITCH_PIN_POL == 1)
         io_data->vsel_pin.polarity = 1;
 #else
         io_data->vsel_pin.polarity = 0;
@@ -203,10 +202,10 @@ ATTR_WEAK hpm_stat_t board_init_emmc_host_params(sdmmc_host_t *host, SDMMCHOST_T
 #endif
     }
     if (support_pwr) {
-#if defined(EMMC_POWER_SWITCH_USING_GPIO) && (EMMC_POWER_SWITCH_USING_GPIO == 1)
+#if defined(BOARD_APP_EMMC_POWER_SWITCH_USING_GPIO) && (BOARD_APP_EMMC_POWER_SWITCH_USING_GPIO == 1)
         io_data->pwr_pin.use_gpio = true;
-        io_data->pwr_pin.gpio_pin = EMMC_POWER_SWITCH_PIN;
-#if defined(EMMC_POWER_SWITCH_PIN_POL) && (EMMC_POWER_SWITCH_PIN_POL == 1)
+        io_data->pwr_pin.gpio_pin = BOARD_APP_EMMC_POWER_SWITCH_PIN;
+#if defined(BOARD_APP_EMMC_POWER_SWITCH_PIN_POL) && (BOARD_APP_EMMC_POWER_SWITCH_PIN_POL == 1)
         io_data->pwr_pin.polarity = true;
 #else
         io_data->pwr_pin.polarity = false;
@@ -217,169 +216,4 @@ ATTR_WEAK hpm_stat_t board_init_emmc_host_params(sdmmc_host_t *host, SDMMCHOST_T
 #endif
     }
     return status_success;
-}
-
-#if !defined(SDIO_SUPPORT_3V3) && defined(SDCARD_SUPPORT_3V3)
-#define SDIO_SUPPORT_3V3 SDCARD_SUPPORT_3V3
-#endif
-
-#if !defined(SDIO_SUPPORT_1V8) && defined(SDCARD_SUPPORT_1V8)
-#define SDIO_SUPPORT_1V8 SDCARD_SUPPORT_1V8
-#endif
-
-#if !defined(SDIO_SUPPORT_4BIT) && defined(SDCARD_SUPPORT_4BIT)
-#define SDIO_SUPPORT_4BIT SDCARD_SUPPORT_4BIT
-#endif
-
-#if !defined(SDIO_SUPPORT_CARD_DETECTION) && defined(SDCARD_SUPPORT_CARD_DETECTION)
-#define SDIO_SUPPORT_CARD_DETECTION SDCARD_SUPPORT_CARD_DETECTION
-#endif
-
-#if !defined(SDIO_SUPPORT_POWER_SWITCH) && defined(SDCARD_SUPPORT_POWER_SWITCH)
-#define SDIO_SUPPORT_POWER_SWITCH SDCARD_SUPPORT_POWER_SWITCH
-#endif
-
-#if !defined(SDIO_SUPPORT_VOLTAGE_SWITCH) && defined(SDCARD_SUPPORT_VOLTAGE_SWITCH)
-#define SDIO_SUPPORT_VOLTAGE_SWITCH SDCARD_SUPPORT_VOLTAGE_SWITCH
-#endif
-
-#if !defined(SDIO_VOLTAGE_SWITCH_USING_GPIO) && defined(SDCARD_VOLTAGE_SWITCH_USING_GPIO)
-#define SDIO_VOLTAGE_SWITCH_USING_GPIO SDCARD_VOLTAGE_SWITCH_USING_GPIO
-#endif
-
-#if !defined(SDIO_VSEL_PIN) && defined(SDCARD_VSEL_PIN)
-#define SDIO_VSEL_PIN SDCARD_VSEL_PIN
-#endif
-
-#if !defined(SDIO_VOLTAGE_SWITCH_PIN_POL) && defined(SDCARD_VOLTAGE_SWITCH_PIN_POL)
-#define SDIO_VOLTAGE_SWITCH_PIN_POL SDCARD_VOLTAGE_SWITCH_PIN_POL
-#endif
-
-#if !defined(SDIO_CARD_DETECTION_USING_GPIO) && defined(SDCARD_CARD_DETECTION_USING_GPIO)
-#define SDIO_CARD_DETECTION_USING_GPIO SDCARD_CARD_DETECTION_USING_GPIO
-#endif
-
-#if !defined(SDIO_CARD_DETECTION_PIN) && defined(SDCARD_CARD_DETECTION_PIN)
-#define SDIO_CARD_DETECTION_PIN SDCARD_CARD_DETECTION_PIN
-#endif
-
-#if !defined(SDIO_CARD_DETECTION_PIN_POL) && defined(SDCARD_CARD_DETECTION_PIN_POL)
-#define SDIO_CARD_DETECTION_PIN_POL SDCARD_CARD_DETECTION_PIN_POL
-#endif
-
-#if !defined(SDIO_POWER_SWITCH_USING_GPIO) && defined(SDCARD_POWER_SWITCH_USING_GPIO)
-#define SDIO_POWER_SWITCH_USING_GPIO SDCARD_POWER_SWITCH_USING_GPIO
-#endif
-
-#if !defined(SDIO_POWER_SWITCH_PIN_POL) && defined(SDCARD_POWER_SWITCH_PIN_POL)
-#define SDIO_POWER_SWITCH_PIN_POL SDCARD_POWER_SWITCH_PIN_POL
-#endif
-
-#if !defined(SDIO_POWER_SWITCH_PIN) && defined(SDCARD_POWER_SWITCH_PIN)
-#define SDIO_POWER_SWITCH_PIN SDCARD_POWER_SWITCH_PIN
-#endif
-
-
-ATTR_WEAK hpm_stat_t board_init_sdio_host_params(sdmmc_host_t *host, SDMMCHOST_Type *base)
-{
-    sdmmc_host_param_t *param = &host->host_param;
-    param->host_flags = 0;
-
-    sdmmc_io_init_apis_t *init_apis = &host->host_param.io_init_apis;
-    hpm_sdmmc_extra_io_data_t *io_data = &host->host_param.io_data;
-    param->base = base;
-    param->clock_init_func = sd1_configure_clock;
-    param->hart_id = RUN_CORE;
-    param->delay_ms = system_delay_ms;
-
-    init_apis->cd_io_init = sdxc1_cd_pin;
-    init_apis->cmd_io_init = sdxc1_cmd_pin;
-    init_apis->clk_data_io_init = sdxc1_clk_data_pins;
-
-#if defined(SDIO_SUPPORT_3V3) && (SDIO_SUPPORT_3V3 == 1)
-    param->host_flags |= HPM_SDMMC_HOST_SUPPORT_3V3;
-#endif
-    bool support_1v8 = false;
-    bool support_4bit = false;
-    bool support_vsel = false;
-    bool support_pwr = false;
-    bool support_cd = false;
-#if defined(SDIO_SUPPORT_1V8) && (SDIO_SUPPORT_1V8 == 1)
-    param->host_flags |= HPM_SDMMC_HOST_SUPPORT_1V8;
-    support_1v8 = true;
-#endif
-#if defined(SDIO_SUPPORT_4BIT) && (SDIO_SUPPORT_4BIT == 1)
-    param->host_flags |= HPM_SDMMC_HOST_SUPPORT_4BIT;
-    support_4bit = true;
-#endif
-    if (support_1v8 && support_4bit) {
-        param->host_flags |= HPM_SDMMC_HOST_SUPPORT_SDR50 | HPM_SDMMC_HOST_SUPPORT_SDR104;
-        if (sdxc_is_ddr50_supported(base)) {
-            param->host_flags |= HPM_SDMMC_HOST_SUPPORT_DDR;
-        }
-    }
-
-#if defined(SDIO_SUPPORT_CARD_DETECTION) && (SDIO_SUPPORT_CARD_DETECTION == 1)
-    param->host_flags |= HPM_SDMMC_HOST_SUPPORT_CARD_DETECTION;
-    support_cd = true;
-    init_apis->cd_io_init = sdxc1_cd_pin;
-#endif
-
-#if defined(SDIO_SUPPORT_POWER_SWITCH) && (SDIO_SUPPORT_POWER_SWITCH == 1)
-    param->host_flags |= HPM_SDMMC_HOST_SUPPORT_POWER_SWITCH;
-    support_pwr = true;
-    init_apis->pwr_io_init = init_sdxc_pwr_pin;
-#endif
-
-#if defined(SDIO_SUPPORT_VOLTAGE_SWITCH) && (SDIO_SUPPORT_VOLTAGE_SWITCH == 1)
-    param->host_flags |= HPM_SDMMC_HOST_SUPPORT_VOLTAGE_SWITCH;
-    support_vsel = true;
-    init_apis->vsel_io_init = sdxc1_vsel_pin;
-#endif
-
-    if (support_vsel) {
-#if defined(SDIO_VOLTAGE_SWITCH_USING_GPIO) && (SDIO_VOLTAGE_SWITCH_USING_GPIO == 1)
-        io_data->vsel_pin.use_gpio = true;
-        io_data->vsel_pin.gpio_pin = SDIO_VSEL_PIN;
-#if defined(SDIO_VOLTAGE_SWITCH_PIN_POL) && (SDIO_VOLTAGE_SWITCH_PIN_POL == 1)
-        io_data->vsel_pin.polarity = 1;
-#else
-        io_data->vsel_pin.polarity = 0;
-#endif
-#else
-        io_data->vsel_pin.use_gpio = false;
-        param->host_flags |= HPM_SDMMC_HOST_VSEL_IN_IP;
-#endif
-    }
-    if (support_cd) {
-#if defined(SDIO_CARD_DETECTION_USING_GPIO) && (SDIO_CARD_DETECTION_USING_GPIO == 1)
-        io_data->cd_pin.use_gpio = true;
-        io_data->cd_pin.gpio_pin = SDIO_CARD_DETECTION_PIN;
-#if defined(SDIO_CARD_DETECTION_PIN_POL) && (SDIO_CARD_DETECTION_PIN_POL == 1)
-        io_data->cd_pin.polarity = 1;
-#else
-        io_data->cd_pin.polarity = 0;
-#endif
-#else
-        io_data->cd_pin.use_gpio = false;
-        param->host_flags |= HPM_SDMMC_HOST_CD_IN_IP;
-#endif
-    }
-
-    if (support_pwr) {
-#if defined(SDIO_POWER_SWITCH_USING_GPIO) && (SDIO_POWER_SWITCH_USING_GPIO == 1)
-        io_data->pwr_pin.use_gpio = true;
-        io_data->pwr_pin.gpio_pin = SDIO_POWER_SWITCH_PIN;
-#if defined(SDIO_POWER_SWITCH_PIN_POL) && (SDIO_POWER_SWITCH_PIN_POL == 1)
-        io_data->pwr_pin.polarity = true;
-#else
-        io_data->pwr_pin.polarity = false;
-#endif
-#else
-        io_data->pwr_pin.use_gpio = false;
-        param->host_flags |= HPM_SDMMC_HOST_PWR_IN_IP;
-#endif
-    }
-
-    return status_success;
-}
+}

controller_yy_app/remote_controller/rc_rock.c

@@ -3,7 +3,6 @@
 /* 摇杆失联判定 2s */
 #define RC_ROCK_LINK_LOST_TIME_US 2000000
 
-RC_Rock_Data rc_rock;
 
 /**
  * @brief 摇杆数据初始化, 上电时调用

controller_yy_app/software/debug_printf.c

@@ -57,5 +57,4 @@ void _lseek(void)
 }
 
 #endif
-
 #endif

controller_yy_app/software/drv_usart.c

@@ -0,0 +1,918 @@
+#include "drv_usart.h"
+#include "rkfifo.h"
+#include "stm32f4xx.h"
+#include "string.h"
+#include "board.h"
+
+
+static inline int _stm32_uart_rcc_enable(USART_TypeDef *uartx)
+{
+    int ret = 0;
+    switch ((uint32_t)uartx)
+    {
+    case (uint32_t)USART1:
+        RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
+        break;
+    case (uint32_t)USART2:
+        RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
+        break;
+    case (uint32_t)USART3:
+        RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
+        break;
+    case (uint32_t)UART4:
+        RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART4, ENABLE);
+        break;
+    case (uint32_t)UART5:
+        RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE);
+        break;
+    case (uint32_t)USART6:
+        RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART6, ENABLE);
+        break;
+
+    default:
+        ret = -1;
+        break;
+    }
+    return ret;
+}
+
+static inline int _stm32_dma_rcc_enable(DMA_Stream_TypeDef *tx_dma)
+{
+    int ret = 0;
+    switch ((uint32_t)tx_dma)
+    {
+    case (uint32_t)DMA1_Stream0:
+    case (uint32_t)DMA1_Stream1:
+    case (uint32_t)DMA1_Stream2:
+    case (uint32_t)DMA1_Stream3:
+    case (uint32_t)DMA1_Stream4:
+    case (uint32_t)DMA1_Stream5:
+    case (uint32_t)DMA1_Stream6:
+    case (uint32_t)DMA1_Stream7:
+        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
+        break;
+
+    case (uint32_t)DMA2_Stream0:
+    case (uint32_t)DMA2_Stream1:
+    case (uint32_t)DMA2_Stream2:
+    case (uint32_t)DMA2_Stream3:
+    case (uint32_t)DMA2_Stream4:
+    case (uint32_t)DMA2_Stream5:
+    case (uint32_t)DMA2_Stream6:
+    case (uint32_t)DMA2_Stream7:
+        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE);
+        break;
+
+    default:
+        ret = -1;
+        break;
+    }
+
+    return ret;
+}
+
+static inline int _stm32_gpio_rcc_enable(GPIO_TypeDef *port)
+{
+    int ret = 0;
+    switch ((uint32_t)port)
+    {
+    case (uint32_t)GPIOA:
+        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
+        break;
+    case (uint32_t)GPIOB:
+        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
+        break;
+    case (uint32_t)GPIOC:
+        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
+        break;
+    case (uint32_t)GPIOD:
+        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
+        break;
+    case (uint32_t)GPIOE:
+        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
+        break;
+    case (uint32_t)GPIOF:
+        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOF, ENABLE);
+        break;
+    case (uint32_t)GPIOG:
+        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOG, ENABLE);
+        break;
+    case (uint32_t)GPIOH:
+        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOH, ENABLE);
+        break;
+    case (uint32_t)GPIOI:
+        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOI, ENABLE);
+        break;
+    case (uint32_t)GPIOJ:
+        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOJ, ENABLE);
+        break;
+    case (uint32_t)GPIOK:
+        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOK, ENABLE);
+        break;
+    default:
+        ret = -1;
+        break;
+    }
+    return ret;
+}
+
+static inline uint16_t _stm32_get_pinaf(uint16_t pin)
+{
+    uint16_t ret = 0xff;
+    switch (pin)
+    {
+    case GPIO_Pin_0:
+        ret = GPIO_PinSource0;
+        break;
+    case GPIO_Pin_1:
+        ret = GPIO_PinSource1;
+        break;
+    case GPIO_Pin_2:
+        ret = GPIO_PinSource2;
+        break;
+    case GPIO_Pin_3:
+        ret = GPIO_PinSource3;
+        break;
+    case GPIO_Pin_4:
+        ret = GPIO_PinSource4;
+        break;
+    case GPIO_Pin_5:
+        ret = GPIO_PinSource5;
+        break;
+    case GPIO_Pin_6:
+        ret = GPIO_PinSource6;
+        break;
+    case GPIO_Pin_7:
+        ret = GPIO_PinSource7;
+        break;
+    case GPIO_Pin_8:
+        ret = GPIO_PinSource8;
+        break;
+    case GPIO_Pin_9:
+        ret = GPIO_PinSource9;
+        break;
+    case GPIO_Pin_10:
+        ret = GPIO_PinSource10;
+        break;
+    case GPIO_Pin_11:
+        ret = GPIO_PinSource11;
+        break;
+    case GPIO_Pin_12:
+        ret = GPIO_PinSource12;
+        break;
+    case GPIO_Pin_13:
+        ret = GPIO_PinSource13;
+        break;
+    case GPIO_Pin_14:
+        ret = GPIO_PinSource14;
+        break;
+    case GPIO_Pin_15:
+        ret = GPIO_PinSource15;
+        break;
+    default:
+        break;
+    }
+    return ret;
+}
+
+static inline uint8_t _stm32_get_uartaf(USART_TypeDef *uartx)
+{
+    uint8_t ret = 0;
+    switch ((uint32_t)uartx)
+    {
+    case (uint32_t)USART1:
+        ret = GPIO_AF_USART1;
+        break;
+    case (uint32_t)USART2:
+        ret = GPIO_AF_USART2;
+        break;
+    case (uint32_t)USART3:
+        ret = GPIO_AF_USART3;
+        break;
+    case (uint32_t)UART4:
+        ret = GPIO_AF_UART4;
+        break;
+    case (uint32_t)UART5:
+        ret = GPIO_AF_UART5;
+        break;
+    case (uint32_t)USART6:
+        ret = GPIO_AF_USART6;
+        break;
+
+    default:
+        ret = -1;
+        break;
+    }
+    return ret;
+}
+
+static int _uart_config(struct stm32_uart_config *uart, uint32_t bps)
+{
+    int ret = 0;
+    _stm32_uart_rcc_enable(uart->uartx);
+    _stm32_gpio_rcc_enable(uart->_tx_port);
+    _stm32_gpio_rcc_enable(uart->_rx_port);
+
+    USART_DeInit(uart->uartx);
+    GPIO_InitTypeDef GPIO_InitStructure;
+    GPIO_StructInit(&GPIO_InitStructure);
+    GPIO_InitStructure.GPIO_Pin = uart->_tx_pin;
+    GPIO_InitStructure.GPIO_Speed = GPIO_High_Speed;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+    GPIO_Init(uart->_tx_port, &GPIO_InitStructure);
+    GPIO_InitStructure.GPIO_Pin = uart->_rx_pin;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+    GPIO_Init(uart->_rx_port, &GPIO_InitStructure);
+
+    GPIO_PinAFConfig(uart->_rx_port, _stm32_get_pinaf(uart->_rx_pin), _stm32_get_uartaf(uart->uartx));
+    GPIO_PinAFConfig(uart->_tx_port, _stm32_get_pinaf(uart->_tx_pin), _stm32_get_uartaf(uart->uartx));
+
+    USART_InitTypeDef USART_InitStructure;
+    USART_StructInit(&USART_InitStructure);
+    USART_InitStructure.USART_BaudRate = bps;
+
+    USART_Init(uart->uartx, &USART_InitStructure);
+
+    if (uart->_rx_dma == NULL)
+    {
+        /* 采用中断式接收 */
+        USART_ITConfig(uart->uartx, USART_IT_RXNE, ENABLE);
+    }
+    else
+    {
+        /* 采用 dma  式接收*/
+        USART_ITConfig(uart->uartx, USART_IT_IDLE, ENABLE);
+    }
+
+    USART_Cmd(uart->uartx, ENABLE);
+
+    return ret;
+}
+
+static int _uart_dma_config(struct stm32_uart_config *uart)
+{
+    int ret = 0;
+    DMA_InitTypeDef dma_conf;
+
+    /* tx dma config */
+    if (uart->_tx_dma && uart->_dma_tx_buff)
+    {
+        _stm32_dma_rcc_enable(uart->_tx_dma);
+        DMA_DeInit(uart->_tx_dma);
+        while (DMA_GetCmdStatus(uart->_tx_dma) != DISABLE)
+        {
+        };
+        DMA_StructInit(&dma_conf);
+        dma_conf.DMA_Channel = uart->_tx_dma_channel;
+        dma_conf.DMA_PeripheralBaseAddr = (u32)(&(uart->uartx->DR));
+        dma_conf.DMA_Memory0BaseAddr = (u32)uart->_dma_tx_buff;
+        dma_conf.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+        dma_conf.DMA_BufferSize = (uint32_t)uart->_dma_tx_buff;
+        dma_conf.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+        dma_conf.DMA_MemoryInc = DMA_MemoryInc_Enable;
+        dma_conf.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+        dma_conf.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte;
+        dma_conf.DMA_Mode = DMA_Mode_Normal;
+        dma_conf.DMA_Priority = DMA_Priority_Medium;
+        dma_conf.DMA_FIFOMode = DMA_FIFOMode_Disable;
+        dma_conf.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
+        dma_conf.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+        dma_conf.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+        DMA_Init(uart->_tx_dma, &dma_conf);
+        DMA_ITConfig(uart->_tx_dma, DMA_IT_TC, ENABLE);
+        DMA_ITConfig(uart->_tx_dma, DMA_IT_TE, ENABLE);
+        DMA_Cmd(uart->_tx_dma, DISABLE);
+        // 串口采用 dma 发送
+        USART_DMACmd(uart->uartx, USART_DMAReq_Tx, ENABLE);
+        //等待关闭使能完成
+        while (DMA_GetCmdStatus(uart->_tx_dma) != DISABLE)
+        {
+        };
+    }
+
+    /* rx dma config */
+    if (uart->_rx_dma && uart->_dma_rx_buff)
+    {
+        // 配置串口的 dma 接收
+        _stm32_dma_rcc_enable(uart->_rx_dma);
+        DMA_StructInit(&dma_conf);
+        DMA_DeInit(uart->_rx_dma);
+        while (DMA_GetCmdStatus(uart->_rx_dma) != DISABLE)
+        {
+        };
+        dma_conf.DMA_Channel = uart->_rx_dma_channel;
+        dma_conf.DMA_PeripheralBaseAddr = (u32)(&uart->uartx->DR);
+        dma_conf.DMA_Memory0BaseAddr = (u32)uart->_dma_rx_buff;
+        dma_conf.DMA_DIR = DMA_DIR_PeripheralToMemory;
+        dma_conf.DMA_BufferSize = uart->_dma_rx_buff_size;
+        dma_conf.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+        dma_conf.DMA_MemoryInc = DMA_MemoryInc_Enable;
+        dma_conf.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+        dma_conf.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte;
+        dma_conf.DMA_Mode = DMA_Mode_Normal;
+        dma_conf.DMA_Priority = DMA_Priority_High;
+        dma_conf.DMA_FIFOMode = DMA_FIFOMode_Disable;
+        dma_conf.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
+        dma_conf.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+        dma_conf.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+        DMA_Init(uart->_rx_dma, &dma_conf);
+        DMA_ITConfig(uart->_rx_dma, DMA_IT_TC, ENABLE);
+        DMA_ITConfig(uart->_rx_dma, DMA_IT_TE, ENABLE);
+        DMA_Cmd(uart->_rx_dma, ENABLE);
+        /* 串口采用 dma 接收 */
+        USART_DMACmd(uart->uartx, USART_DMAReq_Rx, ENABLE);
+        while (DMA_GetCmdStatus(uart->_rx_dma) != ENABLE)
+        {
+        };
+    }
+
+    return ret;
+}
+
+void _uart_nvic_config(struct stm32_uart_config *uart)
+{
+    NVIC_InitTypeDef NVIC_InitStructure;
+
+    NVIC_InitStructure.NVIC_IRQChannel = uart->_uart_irq_channel;
+    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
+    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+    NVIC_Init(&NVIC_InitStructure);
+
+    if (uart->_rx_dma && uart->_dma_rx_buff)
+    {
+        NVIC_InitStructure.NVIC_IRQChannel = uart->_rx_dma_irq_channel;
+        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+        NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
+        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+        NVIC_Init(&NVIC_InitStructure);
+    }
+
+    if (uart->_tx_dma && uart->_dma_tx_buff)
+    {
+        NVIC_InitStructure.NVIC_IRQChannel = uart->_tx_dma_irq_channel;
+        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+        NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
+        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+        NVIC_Init(&NVIC_InitStructure);
+    }
+}
+
+int uart_init(struct stm32_uart_config *uart, uint32_t bps)
+{
+    int ret = 0;
+    rkfifo_init(&uart->_rx_fifo, uart->_rx_fifo_buff, uart->_rx_fifo_buff_size, 1);
+    rkfifo_init(&uart->_tx_fifo, uart->_tx_fifo_buff, uart->_tx_fifo_buff_size, 1);
+
+    _uart_config(uart, bps);
+    _uart_dma_config(uart);
+    _uart_nvic_config(uart);
+
+    return ret;
+}
+
+/**
+ * @brief 串口发送数据
+ *
+ * @param uart 串口对象
+ * @param pdata 发送的数据指针
+ * @param len 发送的数据长度
+ * @return uint32_t 实际发送的数据长度
+ */
+static uint32_t uart_tx_data(struct stm32_uart_config *uart, const void *pdata, uint32_t len)
+{
+    assert_param(uart != NULL);
+    assert_param(pdata != NULL);
+    uint32_t ret = 0;
+    if (len > 0)
+    {
+        /* 将数据压入 tx FIFO */
+        ret = rkfifo_in(&uart->_tx_fifo, pdata, len);
+
+        if (uart->_tx_dma)
+        {
+            /* dma 形式发送数据 */
+            if ((USART_GetFlagStatus(uart->uartx, USART_FLAG_TXE) == SET) &&
+                (USART_GetFlagStatus(uart->uartx, USART_FLAG_TC) == SET))
+            {
+                // fifo 中是否还有数据, 如果还有数据, 则读取到 dma buff 中并进行发送
+                uint32_t count = rkfifo_out(&uart->_tx_fifo,
+                                            uart->_dma_tx_buff,
+                                            uart->_dma_tx_buff_size);
+                if (count > 0)
+                {
+                    DMA_SetCurrDataCounter(uart->_tx_dma, count);
+                    DMA_Cmd(uart->_tx_dma, ENABLE);
+                }
+            }
+        }
+        else
+        {
+            /* 中断形式发送数据 */
+            USART_ITConfig(uart->uartx, USART_IT_TXE, ENABLE);
+        }
+    }
+
+    return ret;
+}
+
+/**
+ * @brief 串口读取数据
+ *
+ * @param uart 串口对象
+ * @param pdata 数据读取输出 buffer
+ * @param len 数据读取长度
+ * @return uint32_t 实际读出的长度
+ */
+static uint32_t uart_rx_data(struct stm32_uart_config *uart, void *pdata, uint32_t len)
+{
+    assert_param(uart != NULL);
+    assert_param(pdata != NULL);
+
+    uint32_t ret = rkfifo_out(&uart->_rx_fifo, pdata, len);
+    return ret;
+}
+
+static void _rxne_isr_callback(struct stm32_uart *uart)
+{
+    assert_param(uart != NULL);
+
+    USART_TypeDef *uartx = uart->_config->uartx;
+    rkfifo_t *rxfifo = &uart->_config->_rx_fifo;
+
+    uint8_t data;
+    data = USART_ReceiveData(uartx) & 0xFF;
+    /* 将接收到的数据压入 fifo */
+    rkfifo_in(rxfifo, &data, 1);
+    USART_ClearITPendingBit(uartx, USART_IT_RXNE);
+}
+
+static void _txe_isr_callback(struct stm32_uart *uart)
+{
+    assert_param(uart != NULL);
+
+    USART_TypeDef *usartx = uart->_config->uartx;
+    rkfifo_t *txfifo = &uart->_config->_tx_fifo;
+
+    if (USART_GetITStatus(usartx, USART_IT_TXE) == SET)
+    {
+        uint8_t c;
+        if (rkfifo_out(txfifo, &c, 1))
+        {
+            /* fifo 有数据, 继续发送 */
+            USART_SendData(usartx, c);
+        }
+        else
+        {
+            /* fifo 无数据, 关闭 txe 中断, 打开 TC 中断 */
+            USART_ITConfig(usartx, USART_IT_TXE, DISABLE);
+            USART_ITConfig(usartx, USART_IT_TC, ENABLE);
+        }
+
+        USART_ClearITPendingBit(usartx, USART_IT_TXE);
+    }
+}
+
+static void _tc_isr_callback(struct stm32_uart *uart)
+{
+    assert_param(uart != NULL);
+
+    USART_TypeDef *usartx = uart->_config->uartx;
+
+    USART_ClearITPendingBit(usartx, USART_IT_TC);
+}
+
+static void _uart_isr_callback(struct stm32_uart *uart)
+{
+    USART_TypeDef *uartx = uart->_config->uartx;
+    DMA_Stream_TypeDef *rx_dma = uart->_config->_rx_dma;
+    uint8_t *rx_dma_buff = uart->_config->_dma_rx_buff;
+    uint32_t rx_dma_buff_size = uart->_config->_dma_rx_buff_size;
+    rkfifo_t *rx_fifo = &uart->_config->_rx_fifo;
+
+    if (USART_GetITStatus(uartx, USART_IT_RXNE) == SET)
+    {
+        _rxne_isr_callback(uart);
+    }
+    else if (USART_GetITStatus(uartx, USART_IT_TXE) == SET)
+    {
+        _txe_isr_callback(uart);
+    }
+    else if (USART_GetITStatus(uartx, USART_IT_TC) == SET)
+    {
+        _tc_isr_callback(uart);
+    }
+    else if (USART_GetITStatus(uartx, USART_IT_IDLE) == SET)
+    {
+        if (rx_dma)
+        {
+            DMA_Cmd(rx_dma, DISABLE);
+            uint32_t dma_cnt = rx_dma_buff_size -
+                               DMA_GetCurrDataCounter(rx_dma);
+            if (dma_cnt)
+            {
+                rkfifo_in(rx_fifo, &rx_dma_buff[0], dma_cnt);
+            }
+            //重新设置传输数据长度
+            DMA_SetCurrDataCounter(rx_dma, uart->_config->_dma_rx_buff_size);
+            //打开DMA
+            DMA_Cmd(rx_dma, ENABLE);
+        }
+
+        uartx->SR;
+        uartx->DR;
+    }
+    else
+    {
+        uartx->SR;
+        uartx->DR;
+    }
+}
+
+static void _uart_tx_dma_isr_callback(struct stm32_uart *uart)
+{
+    DMA_Stream_TypeDef *tx_dma = uart->_config->_tx_dma;
+    rkfifo_t *tx_fifo = &uart->_config->_tx_fifo;
+    uint32_t tcif = uart->_config->_tx_dma_tcif;
+    uint32_t teif = uart->_config->_tx_dma_teif;
+
+    if (DMA_GetITStatus(tx_dma, tcif) == SET)
+    {
+        DMA_ClearITPendingBit(tx_dma, tcif);
+        DMA_Cmd(tx_dma, DISABLE);
+
+        // fifo 中是否还有数据, 如果还有数据, 则读取到 dma buff 中并进行发送
+        uint8_t *dma_buff = uart->_config->_dma_tx_buff;
+        uint32_t dma_buff_size = uart->_config->_dma_tx_buff_size;
+        uint32_t count = rkfifo_out(tx_fifo, dma_buff, dma_buff_size);
+        if (count > 0)
+        {
+            DMA_SetCurrDataCounter(tx_dma, count);
+            DMA_Cmd(tx_dma, ENABLE);
+        }
+        else
+        {
+            // 数据发送完成，在多线程时可以抛出信号量
+        }
+    }
+    else if (DMA_GetITStatus(tx_dma, teif) == SET)
+    {
+        DMA_ClearITPendingBit(tx_dma, teif);
+    }
+}
+
+static void _uart_rx_dma_isr_callback(struct stm32_uart *uart)
+{
+    DMA_Stream_TypeDef *rx_dma = uart->_config->_rx_dma;
+    rkfifo_t *rx_fifo = &uart->_config->_rx_fifo;
+    uint32_t tcif = uart->_config->_rx_dma_tcif;
+    uint32_t teif = uart->_config->_rx_dma_teif;
+
+    if (DMA_GetITStatus(rx_dma, tcif) == SET)
+    {
+        DMA_ClearITPendingBit(rx_dma, tcif);
+        DMA_Cmd(rx_dma, DISABLE);
+
+        uint8_t *dma_buff = uart->_config->_dma_rx_buff;
+        uint32_t dma_buff_size = uart->_config->_dma_rx_buff_size;
+
+        uint32_t dma_cnt = dma_buff_size - DMA_GetCurrDataCounter(rx_dma);
+        if (dma_cnt)
+        {
+            rkfifo_in(rx_fifo, &dma_buff[0], dma_cnt);
+        }
+        //重新设置传输数据长度
+        DMA_SetCurrDataCounter(rx_dma, dma_buff_size);
+        //打开DMA
+        DMA_Cmd(rx_dma, ENABLE);
+    }
+    else if (DMA_GetITStatus(rx_dma, teif) == SET)
+    {
+        DMA_ClearITPendingBit(rx_dma, teif);
+    }
+}
+/**--------------------------- UART1 -----------------------------------------*/
+#ifdef DRV_USING_UART1 
+static uint8_t u1_rx_fifo_buff[UART1_RX_FIFO_BUFFER_LEN] = {0};
+static uint8_t u1_tx_fifo_buff[UART1_TX_FIFO_BUFFER_LEN] = {0};
+#ifdef UART1_TX_USING_DMA
+static uint8_t usart1_dma_tx_buf[UART1_TX_DMA_BUFFER_LEN] = {0};
+#endif
+#ifdef UART1_RX_USING_DMA
+static uint8_t usart1_dma_rx_buf[UART1_RX_DMA_BUFFER_LEN] = {0};
+#endif
+
+struct stm32_uart_config _u1_config = {
+    .uartx = USART1,
+    ._tx_port = GPIOA,
+    ._tx_pin = GPIO_Pin_9,
+    ._rx_port = GPIOA,
+    ._rx_pin = GPIO_Pin_10,
+    ._uart_irq_channel = USART1_IRQn,
+
+    ._tx_fifo_buff = u1_tx_fifo_buff,
+    ._tx_fifo_buff_size = sizeof(u1_tx_fifo_buff),
+    ._rx_fifo_buff = u1_rx_fifo_buff,
+    ._rx_fifo_buff_size = sizeof(u1_rx_fifo_buff),
+
+#ifdef UART1_RX_USING_DMA
+    ._rx_dma = DMA2_Stream2,
+    ._rx_dma_tcif = DMA_IT_TCIF2,
+    ._rx_dma_teif = DMA_IT_TEIF2,
+    ._rx_dma_channel = DMA_Channel_4,
+    ._dma_rx_buff = usart1_dma_rx_buf,
+    ._dma_rx_buff_size = sizeof(usart1_dma_rx_buf),
+    ._rx_dma_irq_channel = DMA2_Stream2_IRQn,
+#else
+    ._rx_dma = NULL,
+#endif // USART1_RX_USING_DMA
+
+#ifdef UART1_TX_USING_DMA
+    ._tx_dma = DMA2_Stream7,
+    ._tx_dma_tcif = DMA_IT_TCIF7,
+    ._tx_dma_teif = DMA_IT_TEIF7,
+    ._tx_dma_channel = DMA_Channel_4,
+    ._dma_tx_buff = usart1_dma_tx_buf,
+    ._dma_tx_buff_size = sizeof(usart1_dma_tx_buf),
+    ._tx_dma_irq_channel = DMA2_Stream7_IRQn,
+#else
+    ._tx_dma = NULL,
+#endif // USART1_TX_USING_DMA
+};
+
+static uint32_t u1_write_data(const void *pdata, uint32_t len)
+{
+    return uart_tx_data(&_u1_config, pdata, len);
+}
+static uint32_t u1_read_data(void *pdata, uint32_t len)
+{
+    return uart_rx_data(&_u1_config, pdata, len);
+}
+static int u1_init(uint32_t bps)
+{
+    return uart_init(&_u1_config, bps);
+}
+
+static struct stm32_uart_ops _u1_ops = {
+    .init = u1_init,
+    .read = u1_read_data,
+    .write = u1_write_data};
+
+static struct stm32_uart uart1 = {
+    ._config = &_u1_config,
+    .ops = &_u1_ops};
+
+void USART1_IRQHandler(void)
+{
+    _uart_isr_callback(&uart1);
+}
+
+#ifdef UART1_TX_USING_DMA
+void DMA2_Stream7_IRQHandler(void)
+{
+    _uart_tx_dma_isr_callback(&uart1);
+}
+#endif
+
+#ifdef UART1_RX_USING_DMA
+void DMA2_Stream2_IRQHandler(void)
+{
+    _uart_rx_dma_isr_callback(&uart1);
+}
+#endif
+#endif // DRV_USING_UART1
+
+/**--------------------------- UART2 -----------------------------------------*/
+#ifdef DRV_USING_UART2 
+static uint8_t u2_rx_fifo_buff[UART2_RX_FIFO_BUFFER_LEN] = {0};
+static uint8_t u2_tx_fifo_buff[UART2_TX_FIFO_BUFFER_LEN] = {0};
+#ifdef UART2_TX_USING_DMA
+static uint8_t usart2_dma_tx_buf[UART2_TX_DMA_BUFFER_LEN] = {0};
+#endif
+#ifdef UART2_RX_USING_DMA
+static uint8_t usart2_dma_rx_buf[UART2_RX_DMA_BUFFER_LEN] = {0};
+#endif
+
+struct stm32_uart_config _u2_config = {
+    .uartx = USART2,
+    ._tx_port = GPIOA,
+    ._tx_pin = GPIO_Pin_2,
+    ._rx_port = GPIOA,
+    ._rx_pin = GPIO_Pin_3,
+    ._uart_irq_channel = USART2_IRQn,
+
+    ._tx_fifo_buff = u2_tx_fifo_buff,
+    ._tx_fifo_buff_size = sizeof(u2_tx_fifo_buff),
+    ._rx_fifo_buff = u2_rx_fifo_buff,
+    ._rx_fifo_buff_size = sizeof(u2_rx_fifo_buff),
+
+#ifdef UART2_RX_USING_DMA
+    ._rx_dma = DMA1_Stream5,
+    ._rx_dma_tcif = DMA_IT_TCIF5,
+    ._rx_dma_teif = DMA_IT_TEIF5,
+    ._rx_dma_channel = DMA_Channel_4,
+    ._dma_rx_buff = usart2_dma_rx_buf,
+    ._dma_rx_buff_size = sizeof(usart2_dma_rx_buf),
+    ._rx_dma_irq_channel = DMA1_Stream5_IRQn,
+#else
+    ._rx_dma = NULL,
+#endif // USART1_RX_USING_DMA
+
+#ifdef UART2_TX_USING_DMA
+    ._tx_dma = DMA1_Stream6,
+    ._tx_dma_tcif = DMA_IT_TCIF6,
+    ._tx_dma_teif = DMA_IT_TEIF6,
+    ._tx_dma_channel = DMA_Channel_4,
+    ._dma_tx_buff = usart2_dma_tx_buf,
+    ._dma_tx_buff_size = sizeof(usart2_dma_tx_buf),
+    ._tx_dma_irq_channel = DMA1_Stream6_IRQn,
+#else
+    ._tx_dma = NULL,
+#endif // USART2_TX_USING_DMA
+};
+
+static uint32_t u2_write_data(const void *pdata, uint32_t len)
+{
+    return uart_tx_data(&_u2_config, pdata, len);
+}
+static uint32_t u2_read_data(void *pdata, uint32_t len)
+{
+    return uart_rx_data(&_u2_config, pdata, len);
+}
+static int u2_init(uint32_t bps)
+{
+    return uart_init(&_u2_config, bps);
+}
+
+static struct stm32_uart_ops _u2_ops = {
+    .init = u2_init,
+    .read = u2_read_data,
+    .write = u2_write_data};
+
+static struct stm32_uart uart2 = {
+    ._config = &_u2_config,
+    .ops = &_u2_ops};
+
+void USART2_IRQHandler(void)
+{
+    _uart_isr_callback(&uart2);
+}
+
+#ifdef UART2_TX_USING_DMA
+void DMA1_Stream6_IRQHandler(void)
+{
+    _uart_tx_dma_isr_callback(&uart1);
+}
+#endif
+
+#ifdef UART2_RX_USING_DMA
+void DMA1_Stream5_IRQHandler(void)
+{
+    _uart_rx_dma_isr_callback(&uart1);
+}
+#endif
+#endif // DRV_USING_UART2
+
+/**--------------------------- UART3 -----------------------------------------*/
+
+
+/**--------------------------- UART4 -----------------------------------------*/
+#ifdef DRV_USING_UART4
+static uint8_t u4_rx_fifo_buff[UART4_RX_FIFO_BUFFER_LEN] = {0};
+static uint8_t u4_tx_fifo_buff[UART4_TX_FIFO_BUFFER_LEN] = {0};
+#ifdef UART4_TX_USING_DMA
+static uint8_t u4_dma_tx_buf[UART4_TX_DMA_BUFFER_LEN] = {0};
+#endif
+#ifdef UART4_RX_USING_DMA
+static uint8_t u4_dma_rx_buf[UART4_RX_DMA_BUFFER_LEN] = {0};
+#endif
+
+struct stm32_uart_config _u4_config = {
+    .uartx = UART4,
+    ._tx_port = GPIOA,
+    ._tx_pin = GPIO_Pin_0,
+    ._rx_port = GPIOA,
+    ._rx_pin = GPIO_Pin_1,
+    ._uart_irq_channel = UART4_IRQn,
+
+    ._tx_fifo_buff = u4_tx_fifo_buff,
+    ._tx_fifo_buff_size = sizeof(u4_tx_fifo_buff),
+    ._rx_fifo_buff = u4_rx_fifo_buff,
+    ._rx_fifo_buff_size = sizeof(u4_rx_fifo_buff),
+
+#ifdef UART4_RX_USING_DMA
+    ._rx_dma = DMA1_Stream2,
+    ._rx_dma_tcif = DMA_IT_TCIF2,
+    ._rx_dma_teif = DMA_IT_TEIF2,
+    ._rx_dma_channel = DMA_Channel_4,
+    ._dma_rx_buff = u4_dma_rx_buf,
+    ._dma_rx_buff_size = sizeof(u4_dma_rx_buf),
+    ._rx_dma_irq_channel = DMA1_Stream2_IRQn,
+#else
+    ._rx_dma = NULL,
+#endif
+
+#ifdef UART4_TX_USING_DMA
+    ._tx_dma = DMA1_Stream4,
+    ._tx_dma_tcif = DMA_IT_TCIF4,
+    ._tx_dma_teif = DMA_IT_TEIF4,
+    ._tx_dma_channel = DMA_Channel_4,
+    ._dma_tx_buff = u4_dma_tx_buf,
+    ._dma_tx_buff_size = sizeof(u4_dma_tx_buf),
+    ._tx_dma_irq_channel = DMA1_Stream4_IRQn,
+#else
+    ._tx_dma = NULL,
+#endif
+};
+
+static uint32_t u4_write_data(const void *pdata, uint32_t len)
+{
+    return uart_tx_data(&_u4_config, pdata, len);
+}
+static uint32_t u4_read_data(void *pdata, uint32_t len)
+{
+    return uart_rx_data(&_u4_config, pdata, len);
+}
+static int u4_init(uint32_t bps)
+{
+    return uart_init(&_u4_config, bps);
+}
+
+static struct stm32_uart_ops _u4_ops = {
+    .init = u4_init,
+    .read = u4_read_data,
+    .write = u4_write_data};
+
+static struct stm32_uart uart4 = {
+    ._config = &_u4_config,
+    .ops = &_u4_ops};
+
+void UART4_IRQHandler(void)
+{
+    _uart_isr_callback(&uart4);
+}
+
+#ifdef UART4_RX_USING_DMA
+void DMA1_Stream2_IRQHandler(void)
+{
+    _uart_rx_dma_isr_callback(&uart4);
+}
+#endif
+
+#ifdef UART4_TX_USING_DMA
+void DMA1_Stream4_IRQHandler(void)
+{
+    _uart_tx_dma_isr_callback(&uart4);
+}
+#endif
+#endif // DRV_USING_UART4
+
+struct stm32_uart *uart_find(const char *name)
+{
+    struct stm32_uart *uart = NULL;
+    if (strncmp(name, "uart1", 5) == 0)
+    {
+#ifdef DRV_USING_UART1
+        uart = &uart1;
+#endif // DEBUG
+    }
+    else if (strncmp(name, "uart2", 5) == 0)
+    {
+#ifdef DRV_USING_UART2
+        uart = &uart2;
+#endif // DEBUG
+    }
+    else if (strncmp(name, "uart3", 5) == 0)
+    {
+#ifdef DRV_USING_UART3
+        uart = &uart3;
+#endif // DEBUG
+    }
+    else if (strncmp(name, "uart4", 5) == 0)
+    {
+#ifdef DRV_USING_UART4
+        uart = &uart4;
+#endif // DEBUG
+    }
+    else if (strncmp(name, "uart5", 5) == 0)
+    {
+#ifdef DRV_USING_UART5
+        uart = &uart5;
+#endif // DEBUG
+    }
+    else if (strncmp(name, "uart6", 5) == 0)
+    {
+#ifdef DRV_USING_UART6
+        uart = &uart6;
+#endif // DEBUG
+    }
+    else
+    {
+        uart = NULL;
+    }
+
+    return uart;
+}

controller_yy_app/software/my_crc.c

@@ -0,0 +1,48 @@
+#include "my_crc.h"
+
+static const uint16_t crc16_table[] = {
+    0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241, 0xC601,
+    0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440, 0xCC01, 0x0CC0,
+    0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40, 0x0A00, 0xCAC1, 0xCB81,
+    0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841, 0xD801, 0x18C0, 0x1980, 0xD941,
+    0x1B00, 0xDBC1, 0xDA81, 0x1A40, 0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01,
+    0x1DC0, 0x1C80, 0xDC41, 0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0,
+    0x1680, 0xD641, 0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081,
+    0x1040, 0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
+    0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441, 0x3C00,
+    0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41, 0xFA01, 0x3AC0,
+    0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840, 0x2800, 0xE8C1, 0xE981,
+    0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41, 0xEE01, 0x2EC0, 0x2F80, 0xEF41,
+    0x2D00, 0xEDC1, 0xEC81, 0x2C40, 0xE401, 0x24C0, 0x2580, 0xE541, 0x2700,
+    0xE7C1, 0xE681, 0x2640, 0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0,
+    0x2080, 0xE041, 0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281,
+    0x6240, 0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
+    0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41, 0xAA01,
+    0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840, 0x7800, 0xB8C1,
+    0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41, 0xBE01, 0x7EC0, 0x7F80,
+    0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40, 0xB401, 0x74C0, 0x7580, 0xB541,
+    0x7700, 0xB7C1, 0xB681, 0x7640, 0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101,
+    0x71C0, 0x7080, 0xB041, 0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0,
+    0x5280, 0x9241, 0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481,
+    0x5440, 0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
+    0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841, 0x8801,
+    0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40, 0x4E00, 0x8EC1,
+    0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, 0x4400, 0x84C1, 0x8581,
+    0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, 0x8201, 0x42C0, 0x4380, 0x8341,
+    0x4100, 0x81C1, 0x8081, 0x4040};
+
+uint16_t crc16_cyc_cal(uint16_t init_val, uint8_t *pdata, uint32_t len)
+{
+    uint32_t i;
+    uint16_t crc16;
+    uint8_t idx;
+
+    crc16 = init_val;
+    for (i = 0; i < len; i++)
+    {
+        idx = ((uint8_t)crc16) ^ (*pdata++);
+        crc16 = (crc16 >> 8) ^ crc16_table[idx];
+    }
+
+    return (crc16);
+}

controller_yy_app/software/soft_can.c

@@ -1,4 +1,3 @@
-
 #include "board.h"
 #include "soft_can.h"
 #include "hard_can.h"
@@ -8,11 +7,12 @@
 #include "soft_time.h"
 #include "ver_config.h"
 #include "soft_can_yy.h"
-
-#define CAN_DEV BOARD_APP_CAN_BASE
-#define CAN_IRQn BOARD_APP_CAN_IRQn
-
-
+// 宏定义与hard can同步修改
+#define CAN2         HPM_CAN2
+#define CAN2_IRQ     IRQn_CAN2
+#define CAN2_CLK_NAME clock_can2
+#define BAUD_1000k   (1000000)
+#define BAUD_500k    (500000)
 
 #define CAN_RX_FIFO_SIZE 16
 #define CAN_TX_FIFO_SIZE 16
@@ -67,7 +67,7 @@ void CAN_FilterConfig(void)
 int canSendMsg(can_transmit_buf_t *pTxMsg)
 {
     int ret = 0;
-    if (can_send_message_nonblocking(CAN_DEV, pTxMsg) != status_success ) // 发送失败
+    if (can_send_message_nonblocking(CAN2, pTxMsg) != status_success ) // 发送失败
     {
         if (rkfifo_in(&canTxFifo, pTxMsg, 1) != 1)
         {
@@ -161,17 +161,17 @@ void CanTxRxServicePoll(void)
 }
 static volatile bool has_sent_out;
 
-SDK_DECLARE_EXT_ISR_M(CAN_IRQn, can0_isr)
-void can0_isr(void)
+SDK_DECLARE_EXT_ISR_M(CAN2_IRQ, can2_isr)
+void can2_isr(void)
 {
     can_transmit_buf_t can_TxMessage;
     can_receive_buf_t can_RxMessage;
-    uint8_t flags = can_get_tx_rx_flags(CAN_DEV);
+    uint8_t flags = can_get_tx_rx_flags(CAN2);
     
     /* 处理接收中断 */
     if ((flags & CAN_EVENT_RECEIVE) != 0) {
         /* 直接读取到应用层消息结构（现在就是HPM格式） */
-        can_read_received_message(CAN_DEV, &can_RxMessage);
+        can_read_received_message(CAN2, &can_RxMessage);
         
         /* 存入FIFO */
         rkfifo_in(&canRxFifo, &can_RxMessage, 1);
@@ -185,20 +185,17 @@ void can0_isr(void)
         /* 检查发送FIFO中是否有待发送消息 */
         if (rkfifo_out(&canTxFifo, &can_TxMessage, 1) == 1) {
             /* 直接发送（已经是HPM格式） */
-            can_send_message_nonblocking(CAN_DEV, &can_TxMessage);
+            can_send_message_nonblocking(CAN2, &can_TxMessage);
         }
     }
     
     /* 清除发送接收标志 */
-    can_clear_tx_rx_flags(CAN_DEV, flags);
+    can_clear_tx_rx_flags(CAN2, flags);
     
     /* 处理错误中断 */
-    uint8_t error_flags = can_get_error_interrupt_flags(CAN_DEV);
+    uint8_t error_flags = can_get_error_interrupt_flags(CAN2);
     if (error_flags != 0) {
         // 可以在这里处理错误
-        can_clear_error_interrupt_flags(CAN_DEV, error_flags);
+        can_clear_error_interrupt_flags(CAN2, error_flags);
     }
 }
-
-
-

controller_yy_app/software/soft_can_yy.c

@@ -689,4 +689,4 @@ void YY_tx_loop(void)
         send_euler_t = micros();
         send_drone_euler_msg();
     }
-}
+}

controller_yy_app/software/soft_flash.c

@@ -1,5 +1,3 @@
-#include "board.h"
-#include "soft_system.h"
 #include "soft_flash.h"
 #include "auto_pilot.h"
 #include "bsp_V8M_flash.h"
@@ -18,6 +16,8 @@
 #include "soft_rc_input.h"
 #include "soft_time.h"
 #include "ver_config.h"
+#include "bsp_V8M_YY_pwm.h"
+#include "hard_system.h"
 #include <string.h>
 
 #ifdef GD25Q16_FLASH
@@ -832,7 +832,7 @@ void write_par_to_flash(void)
 
         write_iap_flag = false;
         // 升级前失能电平芯片输出，PWM脉宽给0
-        gd25q16_cs_high();
+        PWM_IS_DISABLE();
         for (uint8_t i = 1; i <= params_get_value(ParamNum_APType) / 10; i++)
         {
             set_motor_pwm(i, 0);
@@ -840,10 +840,8 @@ void write_par_to_flash(void)
         delay_ms(200);
 
         // 软件复位
-        system_reset();
-      
-    
+        sys_reset();
     }
 }
 
-//------------------End of File----------------------------
+//------------------End of File----------------------------

controller_yy_app/software/soft_gs.c

@@ -1,5 +1,3 @@
-
-#if 0
 #include "soft_gs.h"
 #include "common.h"
 #include "drv_usart.h"
@@ -226,5 +224,3 @@ void gs_send_payload_msg(struct GCS_Link *pgcs, const void *data, uint32_t len)
         }
     }
 }
-
-#endif

controller_yy_app/software/soft_imu.c

@@ -1,4 +1,5 @@
 #include "soft_imu.h"
+#include "hpm_math.h"
 #include "auto_pilot.h"
 #include "common.h"
 #include "control_attitude.h"
@@ -606,11 +607,11 @@ void get_ins_status(void)
     // 收到IMU数据后闪板载灯
     if (imu_link_status == COMP_NORMAL)
     {
-       v8m_yy_led_toggle(V8M_YY_LED_G);
+        v8m_yy_led_toggle(V8M_YY_LED_G);
     }
     else
     {
-        v8m_yy_led_off(V8M_YY_LED_G);
+        v8m_yy_led_on(V8M_YY_LED_G);
     }
 
     // 收到GPS搜星后闪板载灯
@@ -620,7 +621,7 @@ void get_ins_status(void)
     }
     else if (raw_gps._gps_num > 5)
     {
-         v8m_yy_led_on(V8M_YY_LED_R);
+       v8m_yy_led_on(V8M_YY_LED_R);
     }
     else
     {
@@ -1059,4 +1060,4 @@ void imu_tx_msg_to_gcs(struct GCS_Link *pgcs)
         need_gcs_tx_imu_rx_msg = 0;
         gs_tx_vklink_msg(pgcs, &gcs_tx_imu_msg);
     }
-}
+}

controller_yy_app/software/soft_inc/drv_usart.h

@@ -3,56 +3,56 @@
 
 #include "rkfifo.h"
 #include "stdint.h"
-
-
-
-//struct stm32_uart_config
-//{
-//    USART_TypeDef *uartx;
-//    GPIO_TypeDef *_tx_port;
-//    GPIO_TypeDef *_rx_port;
-//    uint8_t _uart_irq_channel;
-//    uint16_t _tx_pin;
-//    uint16_t _rx_pin;
-
-//    uint8_t *_dma_tx_buff;
-//    uint32_t _dma_tx_buff_size;
-//    uint8_t *_dma_rx_buff;
-//    uint32_t _dma_rx_buff_size;
-
-//    DMA_Stream_TypeDef *_tx_dma;
-//    uint32_t _tx_dma_channel;
-//    uint32_t _tx_dma_tcif;
-//    uint32_t _tx_dma_teif;
-//    uint8_t _tx_dma_irq_channel;
-
-//    DMA_Stream_TypeDef *_rx_dma;
-//    uint32_t _rx_dma_channel;
-//    uint32_t _rx_dma_tcif;
-//    uint32_t _rx_dma_teif;
-//    uint8_t _rx_dma_irq_channel;
-
-//    rkfifo_t _rx_fifo;
-//    uint8_t *_rx_fifo_buff;
-//    uint32_t _rx_fifo_buff_size;
-//    rkfifo_t _tx_fifo;
-//    uint8_t *_tx_fifo_buff;
-//    uint32_t _tx_fifo_buff_size;
-//};
-
-//struct stm32_uart_ops
-//{
-//    int (*init)(uint32_t bps);
-//    uint32_t (*write)(const void *pdata, uint32_t len);
-//    uint32_t (*read)(void *pdata, uint32_t len);
-//};
-
-//struct stm32_uart
-//{
-//    struct stm32_uart_ops *ops;
-//    struct stm32_uart_config *_config;
-//};
-
-//struct stm32_uart *uart_find(const char *name);
-
+#if 0
+
+
+struct stm32_uart_config
+{
+    USART_TypeDef *uartx;
+    GPIO_TypeDef *_tx_port;
+    GPIO_TypeDef *_rx_port;
+    uint8_t _uart_irq_channel;
+    uint16_t _tx_pin;
+    uint16_t _rx_pin;
+
+    uint8_t *_dma_tx_buff;
+    uint32_t _dma_tx_buff_size;
+    uint8_t *_dma_rx_buff;
+    uint32_t _dma_rx_buff_size;
+
+    DMA_Stream_TypeDef *_tx_dma;
+    uint32_t _tx_dma_channel;
+    uint32_t _tx_dma_tcif;
+    uint32_t _tx_dma_teif;
+    uint8_t _tx_dma_irq_channel;
+
+    DMA_Stream_TypeDef *_rx_dma;
+    uint32_t _rx_dma_channel;
+    uint32_t _rx_dma_tcif;
+    uint32_t _rx_dma_teif;
+    uint8_t _rx_dma_irq_channel;
+
+    rkfifo_t _rx_fifo;
+    uint8_t *_rx_fifo_buff;
+    uint32_t _rx_fifo_buff_size;
+    rkfifo_t _tx_fifo;
+    uint8_t *_tx_fifo_buff;
+    uint32_t _tx_fifo_buff_size;
+};
+
+struct stm32_uart_ops
+{
+    int (*init)(uint32_t bps);
+    uint32_t (*write)(const void *pdata, uint32_t len);
+    uint32_t (*read)(void *pdata, uint32_t len);
+};
+
+struct stm32_uart
+{
+    struct stm32_uart_ops *ops;
+    struct stm32_uart_config *_config;
+};
+
+struct stm32_uart *uart_find(const char *name);
+#endif
 #endif

controller_yy_app/software/soft_inc/my_board.h

@@ -0,0 +1,20 @@
+#ifndef __BOARD_H__
+#define __BOARD_H__
+
+#define DRV_USING_UART1
+#define UART1_TX_USING_DMA
+#define UART1_RX_USING_DMA
+#define UART1_TX_FIFO_BUFFER_LEN 512
+#define UART1_RX_FIFO_BUFFER_LEN 512
+#define UART1_TX_DMA_BUFFER_LEN 256
+#define UART1_RX_DMA_BUFFER_LEN 256
+
+#define DRV_USING_UART4
+#define UART4_TX_USING_DMA
+#define UART4_RX_USING_DMA
+#define UART4_TX_FIFO_BUFFER_LEN 512
+#define UART4_RX_FIFO_BUFFER_LEN 512
+#define UART4_TX_DMA_BUFFER_LEN 256
+#define UART4_RX_DMA_BUFFER_LEN 256
+
+#endif // !__BOARD_H__

controller_yy_app/software/soft_inc/my_crc.h

@@ -0,0 +1,8 @@
+#ifndef _MY_CRC_H_
+#define _MY_CRC_H_
+
+#include <stdint.h>
+
+uint16_t crc16_cyc_cal(uint16_t init_val, uint8_t *pdata, uint32_t len);
+
+#endif

controller_yy_app/software/soft_inc/soft_can.h

@@ -109,4 +109,4 @@ int canSendMsg(can_transmit_buf_t *pTxMsg);
 
 void CanTxRxServicePoll(void);
 
-#endif
+#endif

controller_yy_app/software/soft_inc/soft_can_yy.h

@@ -1,9 +1,9 @@
 #ifndef __SOFT_CAN_YY_H_
 #define __SOFT_CAN_YY_H_
 
-// #include "stm32f4xx.h"
-#include <stdint.h>
 
+#include <stdint.h>
+#include "hpm_can_drv.h"
 enum Rotate_State
 {
     ROTATE_DEFAULT = 0x00,
@@ -132,7 +132,7 @@ void updata_cmd_status(enum yy_CAN_cmd_execute_bit bit, bool status);
 
 const struct yy_can_rx_msg_data *yy_can_rx_data_get(void);
 
-// int YY_can_rx_decode(const CanRxMsg *rxmsg);
+int YY_can_rx_decode(const can_receive_buf_t *rxmsg);
 
 void YY_tx_loop(void);
 

controller_yy_app/software/soft_inc/soft_gs.h

@@ -5,7 +5,6 @@
 #include "common.h"
 #include "drv_usart.h"
 #include "rkfifo.h"
-// #include "stm32f4xx.h"
 #include "vklink.h"
 #include <stdbool.h>
 #include <stdint.h>

controller_yy_app/software/soft_motor_output.c

@@ -1,4 +1,5 @@
 #include "soft_motor_output.h"
+#include "hpm_math.h"
 #include "auto_pilot.h"
 #include "bsp_V8M_YY_pwm.h"
 #include "bsp_V8M_pwm.h"

controller_yy_app/software/soft_payload.c

@@ -1,6 +1,8 @@
 #include "soft_payload.h"
 
 
+
+
 int payloadCtlMsgDecode(uint16_t ctlID, void *ctlArg, struct payload* pload)
 {
     uint16_t *pArg = (uint16_t *)ctlArg;

controller_yy_app/software/soft_port_uart4.c

@@ -1,4 +1,3 @@
-#if 0
 #include "soft_port_uart4.h"
 #include "control_attitude.h"
 #include "control_rate.h"
@@ -11,8 +10,10 @@
 #include "soft_imu.h"
 #include "soft_time.h"
 #include "soft_usharpradar.h"
+#include "stm32f4xx.h"
 #include "string.h"
 #include "ver_config.h"
+
 struct stm32_uart *payload_uart = NULL;
 
 // 选择串口 4 设备
@@ -122,4 +123,3 @@ void Port4_Service(void)
         break;
     }
 }
-#endif

controller_yy_app/software/soft_rc_input.c

@@ -32,7 +32,7 @@ short raw_rc_in[RC_INPUT_CH_NUM] = {1500, 1500, 1500, 1500, 1200, 1800, 1800};
  *函数原型:		void rc_input_initial(void)
  *功　　能:	  输入初始化
  *******************************************************************************/
-void rc_input_initial(void) { rc_sbus_init(); }
+void rc_input_initial(void) { rc_sbus_init(SBUS_BPS); }
 
 /**************************实现函数********************************************
 *函数原型:		void get_rc_value(void)
@@ -269,16 +269,16 @@ void sbus_channel_anslysis(uint8_t *sbus_data)
 void recv_rcsbus_data_hookfunction(unsigned int len)
 {
     //中断处理函数
-    if (len == 25 && sbus_rx_buf[0] == 0x0f &&
-        (sbus_rx_buf[24] == 0x00 || sbus_rx_buf[24] == 0x04 ||
-         sbus_rx_buf[24] == 0x14 || sbus_rx_buf[24] == 0x24 ||
-         sbus_rx_buf[24] == 0x34))
+    if (len == 25 && uart2_dma_rx_buf[0] == 0x0f &&
+        (uart2_dma_rx_buf[24] == 0x00 || uart2_dma_rx_buf[24] == 0x04 ||
+         uart2_dma_rx_buf[24] == 0x14 || uart2_dma_rx_buf[24] == 0x24 ||
+         uart2_dma_rx_buf[24] == 0x34))
     {
 
         comp_rc_status = COMP_NORMAL;
         sbus_data_link_lost_time_us = 0;
 
-        sbus_channel_anslysis(sbus_rx_buf);
+        sbus_channel_anslysis(uart2_dma_rx_buf);
     }
 }
 

controller_yy_app/software/soft_sdcard.c

@@ -1,25 +1,32 @@
-#include "board.h"
-#include "hpm_sdmmc_sd.h"
 #include "soft_sdcard.h"
 #include "ff.h"
 #include "hard_sdio_sd.h"
-#include "diskio.h"
 #include "my_math.h"
 #include "soft_delay.h"
 #include "soft_time.h"
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+// 掉hpm的sd fatfs库
+/*
+//==========================连续写入测试=============================
+WriteBuffer字节  写入速度    写固定字节数             写大数据块
+512              475kb       写512字节用时1052us 写2*1024*1024字节用时4310us
+1024             563kb       写1024字节用时1775us 写2*1024*1024字节用时3637us
+2048             1280kb      写2048字节用时1562us 写2*1024*1024字节用时1600us
+4096             2528KB      写4096字节用时1582us     写2*1024*1024字节用时810us
 
+/////////////=============实际测试==非连续写入==写512字节用时平均4500us========问题出在哪里？？？？
 
+实际测试
+正常记录过程中拔掉TF卡不会造成死机。f_write之前会检查系统，如果CARD_ERR会直接返回。且不耗时间，直接返回。
 
 
-/*
-//==========================连续写入测试=============================
-...
-（注释内容保持不变）
-*/
+不连续记录时，每次写512字节用时4000us。写5120字节用时5000us。。。所以猜测写操作的时间主要浪费在了数据发送完成后的写入环节。。
 
+
+*/
+#if 0
 /**
  ******************************************************************************
  *                              定义变量
@@ -46,11 +53,6 @@ const char *pospath = "POS";
 
 static uint8_t _sd_init_ok = 0;
 
-/*========== HPM6750 适配：添加设备定义 ==========*/
-#define SDMMC_BASE BOARD_APP_SDXC_BASE
-#define SDMMC_IRQn BOARD_APP_SDXC_IRQn
-#define DEV_SD 0
-
 uint8_t sdcard_initok(void)
 {
     return _sd_init_ok;
@@ -63,104 +65,91 @@ uint8_t sdcard_initok(void)
  */
 void sdcard_inital(uint8_t init_type)
 {
-    // 硬件初始化 被中间件做了？
-    
-    /*========== HPM6750 适配：等待SD卡插入并初始化 ==========*/
-    while ((disk_initialize(DEV_SD))) // 检测不到SD卡
-    {
-        printf("SD CARD ERR \n");
-        delay_ms(600);
-    }
-
-    /*========== HPM6750 适配：配置中断 ==========*/
-    // intc_m_enable_irq_with_priority(SDMMC_IRQn, 1); 要启动中断？？？
-
-    // SDIO方式挂载文件系统，文件系统挂载时会对SDIO设备初始化
-    /*========== HPM6750 适配：保持空路径 ==========*/
-    res_sd = f_mount(&fs, "", 1);
-
-    /*----------------------- 格式化测试 ---------------------------*/
-    /* 如果没有文件系统就格式化创建创建文件系统 */
-    if (res_sd == FR_NO_FILESYSTEM || init_type == 1)
-    {
-        /* 格式化 */
-        /*========== HPM6750 适配：MKFS_PARM结构体 ==========*/
-        MKFS_PARM mkfs_parm;
-        mkfs_parm.fmt = FM_ANY;
-        mkfs_parm.n_fat = 0;
-        mkfs_parm.align = 0;
-        mkfs_parm.n_root = 0;
-        mkfs_parm.au_size = 0;
-        
-        /* 保持空路径 */
-        res_sd = f_mkfs("", &mkfs_parm, work, sizeof(work));
-
-        if (res_sd == FR_OK)
-        {
-            /* 格式化后，先取消挂载 */
-            res_sd = f_mount(NULL, "", 1);
-            /* 重新挂载	*/
-            res_sd = f_mount(&fs, "", 1);
-        }
-        else
-        {
-            // 格式化失败。》》
-            printf("sd card format fail!\n");
-            while (1)
-                ;
-        }
-    }
-    else if (res_sd != FR_OK)
-    {
-        // SD卡挂载文件系统失败,可能原因：SD卡初始化不成功。\r\n");
-        printf("sd card mount fail!\n");
-        while (1)
-            ;
-    }
-    else
-    {
-        // 文件系统挂载成功，可以进行读写测试\r\n");
-    }
-
-    // 创建文件夹
-    res_sd = f_mkdir(logpath);
-    res_sd = f_mkdir(pospath);
-
-    // 检查是否存在LOG文件
-    f_open(&fnew_data, "LOG/LOG_FLY.DAT", FA_CREATE_NEW);
-    f_close(&fnew_data);
-
-    DWORD nclst;
-    unsigned long free_size = 0;
-    FATFS *fs_p = &fs;
-     // 检查剩余容量
-    res_sd = f_getfree("", &nclst, &fs_p);
-    free_size = nclst * fs_p->csize / 2;
-
-    // 打开log文件夹, 读下一个的文件
-    res_sd = f_opendir(&logdir, logpath);
-    res_sd = f_readdir(&logdir, &fno);
-    // 如果剩余容量小于96MB, 删除部分数据文件, 防止写满
-    while (free_size < 96 * 1024)
-    {
-        if (fno.fname[0] != 'L')
-        {
-            char datafile_path[20] = "";
-            // 拼接文件路径
-            strcat(datafile_path, logpath);
-            strcat(datafile_path, "/");
-            strcat(datafile_path, fno.fname);
-            // 删除一个文件
-            f_unlink(datafile_path);
-        }
-        // 再读一下剩余的容量是否够用
-        res_sd = f_getfree("", &nclst, &fs_p);
-        free_size = nclst * fs_p->csize / 2;
-        // 读下一个文件
-        res_sd = f_readdir(&logdir, &fno);
-    }
-
-    _sd_init_ok = 1;
-    // 关闭文件夹
-    res_sd = f_closedir(&logdir);
-}
+    //while ((SD_Init())) // 检测不到SD卡
+    //{
+    //    printf("SD CARD ERR \n");
+    //    delay_ms(600);
+    //}
+
+    //SDIO_NvicConfig();
+
+    //// SDIO方式挂载文件系统，文件系统挂载时会对SDIO设备初始化
+    //res_sd = f_mount(&fs, "", 1);
+
+    ///*----------------------- 格式化测试 ---------------------------*/
+    ///* 如果没有文件系统就格式化创建创建文件系统 */
+    //if (res_sd == FR_NO_FILESYSTEM || init_type == 1)
+    //{
+    //    /* 格式化 */
+    //    res_sd = f_mkfs("", FM_ANY, 0, work, sizeof(work));
+
+    //    if (res_sd == FR_OK)
+    //    {
+    //        /* 格式化后，先取消挂载 */
+    //        res_sd = f_mount(NULL, "", 1);
+    //        /* 重新挂载	*/
+    //        res_sd = f_mount(&fs, "", 1);
+    //    }
+    //    else
+    //    {
+    //        // 格式化失败。》》
+    //        printf("sd card format fail!\n");
+    //        while (1)
+    //            ;
+    //    }
+    //}
+    //else if (res_sd != FR_OK)
+    //{
+    //    // SD卡挂载文件系统失败,可能原因：SD卡初始化不成功。\r\n");
+    //    printf("sd card mount fail!\n");
+    //    while (1)
+    //        ;
+    //}
+    //else
+    //{
+    //    // 文件系统挂载成功，可以进行读写测试\r\n");
+    //}
+
+    //// 创建文件夹
+    //res_sd = f_mkdir(logpath);
+    //res_sd = f_mkdir(pospath);
+
+    //// 检查是否存在LOG文件
+    //f_open(&fnew_data, "LOG/LOG_FLY.DAT", FA_CREATE_NEW);
+    //f_close(&fnew_data);
+
+    //DWORD nclst;
+    //unsigned long free_size = 0;
+    //FATFS *fs_p = &fs;
+    //// 检查剩余容量
+    //res_sd = f_getfree("", &nclst, &fs_p);
+    //free_size = nclst * fs_p->csize / 2;
+
+    //// 打开log文件夹, 读下一个的文件
+    //res_sd = f_opendir(&logdir, logpath);
+    //res_sd = f_readdir(&logdir, &fno);
+    //// 如果剩余容量小于96MB, 删除部分数据文件, 防止写满
+    //while (free_size < 96 * 1024)
+    //{
+    //    if (fno.fname[0] != 'L')
+    //    {
+    //        char datafile_path[20] = "";
+    //        // 拼接文件路径
+    //        strcat(datafile_path, logpath);
+    //        strcat(datafile_path, "/");
+    //        strcat(datafile_path, fno.fname);
+    //        // 删除一个文件
+    //        f_unlink(datafile_path);
+    //    }
+    //    // 再读一下剩余的容量是否够用
+    //    res_sd = f_getfree("", &nclst, &fs_p);
+    //    free_size = nclst * fs_p->csize / 2;
+    //    // 读下一个文件
+    //    res_sd = f_readdir(&logdir, &fno);
+    //}
+
+    //_sd_init_ok = 1;
+    //// 关闭文件夹
+    //res_sd = f_closedir(&logdir);
+}
+#endif

controller_yy_app/software/soft_system.c

@@ -6,14 +6,12 @@
 
 void system_reset(void)
 {
-    sys_reset();  
-    // 引脚复位默认为全局复位，用户可以通过把 POR_CONFIG 寄存器的 RETENTION 位置 1，引脚复位只复位
-    // 电源管理域和系统电源域，不影响电池备份域的状态
+    sys_reset();
 }
 
 
 void close_imu_dma(void)
 {
-    u3_dma_disable();
+    u3_dma_disable(); // 实际dma通道未使用 使用串口fifo中断替代了 实在要用dma 需要触发io物理接在串口接收io 消耗定时器的2路通道 软件模拟空闲中断
 }
 

controller_yy_app/user_src/main.c

@@ -1,3 +1,98 @@
+///*
+// * Copyright (c) 2021 HPMicro
+// *
+// * SPDX-License-Identifier: BSD-3-Clause
+// *
+// */
+
+//#include <stdio.h>
+//#include "board.h"
+
+//#include "test.h"
+//#ifdef TEST_EN
+//#include "bsp_V8M_YY_led.h"
+//#include "bsp_V8M_YY_pwm.h"
+// #include "bsp_V8M_YY_adc.h"
+//#include "hard_system.h"
+//#include "hard_system_time.h"
+//#include "hard_system_delay.h"
+//#include "hard_system_timer.h"
+//#include "hard_imu_uart3.h"
+//#include "hard_rc_sbus.h"
+//#include "hard_can.h"
+//#include "hard_sbus_out.h"
+//#include "main.h"
+//#include "hpm_math.h"
+//#endif
+///*
+//1 手册：中断源优先级，有效值为 0 到 7。
+//2 注意：内存缓存问题 catch
+//  如果DMA要访问​ → 必须用非缓存宏
+//  如果多核要共享​ → 必须用非缓存宏
+//  如果频繁被中断更新​ → 建议用非缓存宏
+//  其他情况​ → 不用修饰，让编译器优化
+//3 注意配置顺序 IO-时钟-外设
+//4 XDMA，作为主设备连接到 AXI 系统总线  HDMA，作为主设备连接到 AHB 外设总线 
+//  当 XDMA 的 destination 为 DRAM 时，如果 burst 大于等于 16，那 destsize 必须为 64bit。
+//  DMAMUX 的输出 0∼7 连接到外设总线 DMA 控制器 HDMA，DMAMUX 的输出 8∼15 连接到系统总线 DMA 控制器 XDMA
+//  它们都连接在统一的 DMAMUX（DMA 多路复用器）
+//  DMAMUX将所有外设的 DMA 请求（Request）统一管理，然后根据你的配置分配给 HDMA 或 XDMA 的任意空闲通道
+//5 注意cfg文件和一些前置的工程配置文件，可能导致编译出错 运行出错 仿真不了 重点：链接文件、yaml、板级cfg文件
+//*/
+//uint64_t delta_time;
+//#define PI (3.1415926f)
+//void start_time(void)
+//{
+//    delta_time = hpm_csr_get_core_mcycle();
+//}
+
+//uint32_t get_end_time(void)
+//{
+//    delta_time = hpm_csr_get_core_mcycle() - delta_time;
+//    return delta_time;
+//}
+//float theta ;
+//float sin_theta;
+//static void test_hard(void)
+//{
+//    // v8m_yy_led_test();
+//    // v8m_yy_motor_pwm_test();
+//    //  v8m_yy_adc_test();
+//    // timer0_test();
+//    // cpu_delay_test();
+//    // timer1_test();
+//    // can2_test();
+//    // imu_uart3_test();
+//    // uart2_sbus_test();
+//    // system_test();
+//    // sbus_uart2_out_test();
+//    while(1)
+//    {
+//      theta += PI*0.1;
+//      sin_theta = hpm_dsp_sin_f32(theta);
+//      board_delay_ms(200);
+//      printf("sin theta is %f\r\n", sin_theta);
+//    }
+     
+//}
+//int main(void)
+//{
+//    board_init();
+//    float i = 9.8f;
+   
+//    sin_theta = hpm_dsp_sin_f32(theta);
+    
+   
+//    printf("hello world %f\n", i);
+//    printf("sin theta is %f\r\n", sin_theta);
+
+//    test_hard();
+//    return 0;
+//}
+//// DMA 最大4k
+
+
+
 /*
  * Copyright (c) 2021 HPMicro
  *
@@ -5,89 +100,300 @@
  *
  */
 
-#include <stdio.h>
 #include "board.h"
+#include "hpm_sdmmc_sd.h"
+#include "ff.h"
+#include "diskio.h"
 
-#include "test.h"
-#ifdef TEST_EN
-#include "bsp_V8M_YY_led.h"
-#include "bsp_V8M_YY_pwm.h"
- #include "bsp_V8M_YY_adc.h"
-#include "hard_system.h"
-#include "hard_system_time.h"
-#include "hard_system_delay.h"
-#include "hard_system_timer.h"
-#include "hard_imu_uart3.h"
-#include "hard_rc_sbus.h"
-#include "hard_can.h"
-#include "hard_sbus_out.h"
-#include "main.h"
-#include "hpm_math.h"
-#endif
-/*
-1 手册：中断源优先级，有效值为 0 到 7。
-2 注意：内存缓存问题 catch
-  如果DMA要访问​ → 必须用非缓存宏
-  如果多核要共享​ → 必须用非缓存宏
-  如果频繁被中断更新​ → 建议用非缓存宏
-  其他情况​ → 不用修饰，让编译器优化
-3 注意配置顺序 IO-时钟-外设
-4 XDMA，作为主设备连接到 AXI 系统总线  HDMA，作为主设备连接到 AHB 外设总线 
-  当 XDMA 的 destination 为 DRAM 时，如果 burst 大于等于 16，那 destsize 必须为 64bit。
-  DMAMUX 的输出 0∼7 连接到外设总线 DMA 控制器 HDMA，DMAMUX 的输出 8∼15 连接到系统总线 DMA 控制器 XDMA
-  它们都连接在统一的 DMAMUX（DMA 多路复用器）
-  DMAMUX将所有外设的 DMA 请求（Request）统一管理，然后根据你的配置分配给 HDMA 或 XDMA 的任意空闲通道
-5 注意cfg文件和一些前置的工程配置文件，可能导致编译出错 运行出错 仿真不了 重点：链接文件、yaml、板级cfg文件
-*/
-uint64_t delta_time;
-#define PI (3.1415926f)
-void start_time(void)
+FATFS s_sd_disk;
+FIL s_file;
+DIR s_dir;
+FRESULT fatfs_result;
+BYTE work[FF_MAX_SS];
+
+const TCHAR driver_num_buf[4] = { DEV_SD + '0', ':', '/', '\0' };
+
+#define TEST_DIR_NAME "hpmicro_sd_test_dir0"
+
+void show_menu(void);
+
+const char *show_error_string(FRESULT fresult);
+
+static FRESULT sd_mount_fs(void);
+
+static FRESULT sd_mkfs(void);
+
+static FRESULT sd_write_file(void);
+
+static FRESULT sd_read_file(void);
+
+static FRESULT sd_dir_test(void);
+
+static FRESULT sd_big_file_test(void);
+
+int main(void)
 {
-    delta_time = hpm_csr_get_core_mcycle();
+    bool need_init_filesystem = true;
+    board_init();
+    show_menu();
+
+    while (1) {
+        char option = getchar();
+
+        /* Before doing FATFS operation, ensure the SD card is present */
+        DSTATUS dstatus = disk_status(DEV_SD);
+        if (dstatus == STA_NODISK) {
+            printf("No disk in the SD slot, please insert an SD card...\n");
+            do {
+                dstatus = disk_status(DEV_SD);
+            } while (dstatus == STA_NODISK);
+            board_delay_ms(100);
+            printf("Detected SD card, re-initialize the filesystem...\n");
+            need_init_filesystem = true;
+        }
+        dstatus = disk_initialize(DEV_SD);
+        if (dstatus != RES_OK) {
+            printf("Failed to initialize SD disk\n");
+        }
+        if (need_init_filesystem) {
+            fatfs_result = sd_mount_fs();
+            if (fatfs_result == FR_NO_FILESYSTEM) {
+                printf("There is no File system available, making file system...\n");
+                fatfs_result = sd_mkfs();
+                if (fatfs_result != FR_OK) {
+                    printf("Failed to make filesystem, cause:%s\n", show_error_string(fatfs_result));
+                } else {
+                    need_init_filesystem = false;
+                }
+            }
+        }
+
+        switch (option) {
+        case '1':
+            fatfs_result = sd_mkfs();
+            break;
+        case '2':
+            fatfs_result = sd_write_file();
+            break;
+        case '3':
+            fatfs_result = sd_read_file();
+            break;
+        case '4':
+            fatfs_result = sd_dir_test();
+            break;
+        case 's':
+            fatfs_result = sd_big_file_test();
+            break;
+        default:
+            show_menu();
+            break;
+        }
+    }
+
 }
 
-uint32_t get_end_time(void)
+void show_menu(void)
 {
-    delta_time = hpm_csr_get_core_mcycle() - delta_time;
-    return delta_time;
+    const char menu_str[] = "SD FATFS demo\n-----------------------------------\n"
+                            "1 - Format the SD card with FATFS\n"
+                            "2 - Create hello.txt\n"
+                            "3 - Read 1st line from hello.txt\n"
+                            "4 - Directory related test\n"
+                            "s - Large file write test\n"
+                            "Others - Show menu\n";
+
+    printf(menu_str);
 }
-float theta ;
-float sin_theta;
-static void test_hard(void)
+
+static FRESULT sd_mount_fs(void)
 {
-    // v8m_yy_led_test();
-    // v8m_yy_motor_pwm_test();
-    //  v8m_yy_adc_test();
-    // timer0_test();
-    // cpu_delay_test();
-    // timer1_test();
-    // can2_test();
-    // imu_uart3_test();
-    // uart2_sbus_test();
-    // system_test();
-    // sbus_uart2_out_test();
-    while(1)
-    {
-      theta += PI*0.1;
-      sin_theta = hpm_dsp_sin_f32(theta);
-      board_delay_ms(200);
-      printf("sin theta is %f\r\n", sin_theta);
+    FRESULT fresult = f_mount(&s_sd_disk, driver_num_buf, 1);
+    if (fresult == FR_OK) {
+        printf("SD card has been mounted successfully\n");
+    } else {
+        printf("Failed to mount SD card, cause: %s\n", show_error_string(fresult));
     }
-     
+
+    fresult = f_chdrive(driver_num_buf);
+    return fresult;
 }
-int main(void)
+
+static FRESULT sd_mkfs(void)
 {
-    board_init();
-    float i = 9.8f;
-   
-    sin_theta = hpm_dsp_sin_f32(theta);
-    
-   
-    printf("hello world %f\n", i);
-    printf("sin theta is %f\r\n", sin_theta);
+    printf("Formatting the SD card, depending on the SD card capacity, the formatting process may take a long time\n");
+    FRESULT fresult = f_mkfs(driver_num_buf, NULL, work, sizeof(work));
+    if (fresult != FR_OK) {
+        printf("Making File system failed, cause: %s\n", show_error_string(fresult));
+    } else {
+        printf("Making file system is successful\n");
+    }
 
-    test_hard();
-    return 0;
+    return fresult;
 }
-//// DMA 最大4k
 
+static FRESULT sd_write_file(void)
+{
+    FRESULT fresult = f_open(&s_file, "readme.txt", FA_WRITE | FA_CREATE_ALWAYS);
+    if (fresult != FR_OK) {
+        printf("Create new file failed, cause: %d\n", show_error_string(fresult));
+    } else {
+        printf("Create new file successfully, status=%d\n", fresult);
+    }
+    char hello_str[] = "Hello, this is SD card FATFS demo\n";
+    UINT byte_written;
+    fresult = f_write(&s_file, hello_str, sizeof(hello_str), &byte_written);
+    if (fresult != FR_OK) {
+        printf("Write file failed, cause: %s\n", show_error_string(fresult));
+    } else {
+        printf("Write file operation is successfully\n");
+    }
+
+    f_close(&s_file);
+
+    return fresult;
+}
+
+static FRESULT sd_read_file(void)
+{
+    FRESULT fresult = f_open(&s_file, "readme.txt", FA_READ);
+    if (fresult != FR_OK) {
+        printf("Open file failed, cause: %s\n", show_error_string(fresult));
+    } else {
+        printf("Open file successfully\n");
+    }
+
+    if (fresult != FR_OK) {
+        return fresult;
+    }
+
+    TCHAR str[100];
+    f_gets(str, sizeof(str), &s_file);
+    printf("%s\n", str);
+
+    f_close(&s_file);
+
+    return fresult;
+}
+
+static FRESULT sd_big_file_test(void)
+{
+    FRESULT fresult = f_open(&s_file, "big_file.bin", FA_WRITE | FA_CREATE_ALWAYS);
+    if (fresult != FR_OK) {
+        printf("Create new file failed, cause: %s\n", show_error_string(fresult));
+    } else {
+        printf("Create new file successfully\n");
+    }
+
+    uint32_t write_size = 1024UL * 1024UL * 100UL;
+    static uint8_t buf[32768];
+    for (uint32_t i = 0; i < sizeof(buf); i++) {
+        buf[i] = i & 0xFF;
+    }
+    while (write_size > 0) {
+        UINT byte_written;
+        fresult = f_write(&s_file, buf, sizeof(buf), &byte_written);
+        if (fresult != FR_OK) {
+            printf("Write file failed, cause: %s\n", show_error_string(fresult));
+            return fresult;
+        }
+
+        write_size -= byte_written;
+    }
+    printf("Write file operation is successful\n");
+
+    f_close(&s_file);
+
+    return fresult;
+}
+
+
+static FRESULT sd_dir_test(void)
+{
+    FRESULT fresult = f_mkdir(TEST_DIR_NAME);
+    if (fresult != FR_OK) {
+        printf("Creating new directory failed, cause: %s\n", show_error_string(fresult));
+    } else {
+        printf("Creating new directory succeeded\n");
+    }
+
+    fresult = f_rmdir(TEST_DIR_NAME);
+    if (fresult != FR_OK) {
+        printf("Removing new directory failed, cause: %s\n", show_error_string(fresult));
+    } else {
+        printf("Removing new directory succeeded\n");
+    }
+
+    return fresult;
+}
+
+const char *show_error_string(FRESULT fresult)
+{
+    const char *result_str;
+
+    switch (fresult) {
+    case FR_OK:
+        result_str = "succeeded";
+        break;
+    case FR_DISK_ERR:
+        result_str = "A hard error occurred in the low level disk I/O level";
+        break;
+    case FR_INT_ERR:
+        result_str = "Assertion failed";
+        break;
+    case FR_NOT_READY:
+        result_str = "The physical drive cannot work";
+        break;
+    case FR_NO_FILE:
+        result_str = "Could not find the file";
+        break;
+    case FR_NO_PATH:
+        result_str = "Could not find the path";
+        break;
+    case FR_INVALID_NAME:
+        result_str = "Tha path name format is invalid";
+        break;
+    case FR_DENIED:
+        result_str = "Access denied due to prohibited access or directory full";
+        break;
+    case FR_EXIST:
+        result_str = "Access denied due to prohibited access";
+        break;
+    case FR_INVALID_OBJECT:
+        result_str = "The file/directory object is invalid";
+        break;
+    case FR_WRITE_PROTECTED:
+        result_str = "The physical drive is write protected";
+        break;
+    case FR_INVALID_DRIVE:
+        result_str = "The logical driver number is invalid";
+        break;
+    case FR_NOT_ENABLED:
+        result_str = "The volume has no work area";
+        break;
+    case FR_NO_FILESYSTEM:
+        result_str = "There is no valid FAT volume";
+        break;
+    case FR_MKFS_ABORTED:
+        result_str = "THe f_mkfs() aborted due to any problem";
+        break;
+    case FR_TIMEOUT:
+        result_str = "Could not get a grant to access the volume within defined period";
+        break;
+    case FR_LOCKED:
+        result_str = "The operation is rejected according to the file sharing policy";
+        break;
+    case FR_NOT_ENOUGH_CORE:
+        result_str = "LFN working buffer could not be allocated";
+        break;
+    case FR_TOO_MANY_OPEN_FILES:
+        result_str = "Number of open files > FF_FS_LOCK";
+        break;
+    case FR_INVALID_PARAMETER:
+        result_str = "Given parameter is invalid";
+        break;
+    default:
+        result_str = "Unknown error";
+        break;
+    }
+    return result_str;
+}

controller_yy_app/v8/v8m/bsp_V8M_GPIO_photo.c

@@ -1,3 +1,4 @@
+#if 0
 #include "bsp_V8M_GPIO_photo.h"
 #include "soft_motor_output.h"
 #include "stm32f4xx.h"
@@ -93,3 +94,4 @@ void V8M_Photo_Gpio_Reset(uint8_t signalType, uint32_t pwmOffValue)
         break;
     }
 }
+#endif

controller_yy_app/v8/v8m/bsp_V8M_flash.c

@@ -1,205 +1,179 @@
-#if 0
+#include "board.h"
+#include "hpm_romapi.h"
+#include "hpm_l1c_drv.h"
+#include "hpm_soc.h"
 #include "bsp_V8M_flash.h"
-#include "stm32f4xx.h"
-
-#define IAP_FLAG_ADDR ((uint32_t)0x0800C000)
-
-#define IPA_FLAG_NEED_UPDATE ((uint16_t)0xABCD)
+// 这里要重新安排，程序什么的所有都要写在外部flash里 且有一些字段是芯片定义的不可写入地区，比如
+// __attribute__ ((section(".nor_cfg_option"))) const uint32_t option[4] = {0xfcf90002, 0x00000007, 0xE, 0x0};
+// define region NOR_CFG_OPTION = [ from 0x80000400 size 0x0C00 ];
+// define region BOOT_HEADER = [ from 0x80001000 size 0x2000 ];
+// 这个字段就是narflash 的描述符，不能覆盖 所以程序得后移
+// 从链接字段里可以发现 define region XPI0 = [from 0x80003000 size (_flash_size - 0x3000) ];   /* XPI0 */ 
+// app是从0x80003000开始的 也就是boot只能从这开始
+#if 0
+// 起始地址 0x80000000
+// IAP标志地址 - 选择Flash中一个合适的扇区
+// HPM6750 Flash通常映射在0x80000000开始
+#define IAP_FLAG_ADDR ((uint32_t)0x8000C000)  
 
-// FLASH扇区的基地址
-#define ADDR_FLASH_SECTOR_0                                                    \
-    ((uint32_t)0x08000000) /* Base @ of Sector 0, 16 Kbyte */
-#define ADDR_FLASH_SECTOR_1                                                    \
-    ((uint32_t)0x08004000) /* Base @ of Sector 1, 16 Kbyte */
-#define ADDR_FLASH_SECTOR_2                                                    \
-    ((uint32_t)0x08008000) /* Base @ of Sector 2, 16 Kbyte */
-#define ADDR_FLASH_SECTOR_3                                                    \
-    ((uint32_t)0x0800C000) /* Base @ of Sector 3, 16 Kbyte */
-#define ADDR_FLASH_SECTOR_4                                                    \
-    ((uint32_t)0x08010000) /* Base @ of Sector 4, 64 Kbyte */
-#define ADDR_FLASH_SECTOR_5                                                    \
-    ((uint32_t)0x08020000) /* Base @ of Sector 5, 128 Kbyte */
-#define ADDR_FLASH_SECTOR_6                                                    \
-    ((uint32_t)0x08040000) /* Base @ of Sector 6, 128 Kbyte */
-#define ADDR_FLASH_SECTOR_7                                                    \
-    ((uint32_t)0x08060000) /* Base @ of Sector 7, 128 Kbyte */
-#define ADDR_FLASH_SECTOR_8                                                    \
-    ((uint32_t)0x08080000) /* Base @ of Sector 8, 128 Kbyte */
-#define ADDR_FLASH_SECTOR_9                                                    \
-    ((uint32_t)0x080A0000) /* Base @ of Sector 9, 128 Kbyte */
-#define ADDR_FLASH_SECTOR_10                                                   \
-    ((uint32_t)0x080C0000) /* Base @ of Sector 10, 128 Kbyte */
-#define ADDR_FLASH_SECTOR_11                                                   \
-    ((uint32_t)0x080E0000) /* Base @ of Sector 11, 128 Kbyte */
+#define IAP_FLAG_NEED_UPDATE ((uint16_t)0xABCD)
 
-// FLASH的结束地址
-#define USER_FLASH_END_ADDRESS 0x080FFFFF
-//用户用来存储APP的FLASH大小
-#define USER_FLASH_SIZE (USER_FLASH_END_ADDRESS - APPLICATION_ADDRESS + 1)
+// Flash大小配置 - 根据实际Flash芯片修改
+#define FLASH_BASE_ADDR         0x80000000
+#define FLASH_SECTOR_SIZE       0x1000      // 4KB，常见QSPI Flash扇区大小
+#define FLASH_PAGE_SIZE         256         // 编程页大小
 
-//用户APP起始地址
-#define APPLICATION_ADDRESS (uint32_t)0x08010000
+// XPI Nor Flash 配置结构体
+static xpi_nor_config_t s_flash_config;
 
 /**
- * @brief 根据 mcu flash 地址计算 所在扇区
+ * @brief 初始化Flash控制器并获取配置
  */
-static uint32_t GetSector(uint32_t Address)
+static void flash_init(void)
 {
-    uint32_t sector = 0;
-
-    if ((Address < ADDR_FLASH_SECTOR_1) && (Address >= ADDR_FLASH_SECTOR_0))
-    {
-        sector = FLASH_Sector_0;
-    }
-    else if ((Address < ADDR_FLASH_SECTOR_2) &&
-             (Address >= ADDR_FLASH_SECTOR_1))
-    {
-        sector = FLASH_Sector_1;
-    }
-    else if ((Address < ADDR_FLASH_SECTOR_3) &&
-             (Address >= ADDR_FLASH_SECTOR_2))
-    {
-        sector = FLASH_Sector_2;
-    }
-    else if ((Address < ADDR_FLASH_SECTOR_4) &&
-             (Address >= ADDR_FLASH_SECTOR_3))
-    {
-        sector = FLASH_Sector_3;
-    }
-    else if ((Address < ADDR_FLASH_SECTOR_5) &&
-             (Address >= ADDR_FLASH_SECTOR_4))
-    {
-        sector = FLASH_Sector_4;
-    }
-    else if ((Address < ADDR_FLASH_SECTOR_6) &&
-             (Address >= ADDR_FLASH_SECTOR_5))
-    {
-        sector = FLASH_Sector_5;
-    }
-    else if ((Address < ADDR_FLASH_SECTOR_7) &&
-             (Address >= ADDR_FLASH_SECTOR_6))
-    {
-        sector = FLASH_Sector_6;
-    }
-    else if ((Address < ADDR_FLASH_SECTOR_8) &&
-             (Address >= ADDR_FLASH_SECTOR_7))
-    {
-        sector = FLASH_Sector_7;
-    }
-    else if ((Address < ADDR_FLASH_SECTOR_9) &&
-             (Address >= ADDR_FLASH_SECTOR_8))
-    {
-        sector = FLASH_Sector_8;
-    }
-    else if ((Address < ADDR_FLASH_SECTOR_10) &&
-             (Address >= ADDR_FLASH_SECTOR_9))
-    {
-        sector = FLASH_Sector_9;
+    static bool s_flash_inited = false;
+    
+    if (!s_flash_inited) {
+        // 获取Flash配置（从ROM API读取）
+        rom_xpi_nor_get_config(HPM_XPI0, &s_flash_config, NULL);
+        s_flash_inited = true;
     }
-    else if ((Address < ADDR_FLASH_SECTOR_11) &&
-             (Address >= ADDR_FLASH_SECTOR_10))
-    {
-        sector = FLASH_Sector_10;
-    }
-    else if ((Address < USER_FLASH_END_ADDRESS) &&
-             (Address >= ADDR_FLASH_SECTOR_11))
-    {
-        sector = FLASH_Sector_11;
-    }
-    return sector;
 }
 
 /**
- * @brief mcu flash 初始化，写入操作前调用，会解锁 mcu flash, 清除标志位
+ * @brief 根据地址计算所在扇区（HPM6750版）
+ * 
+ * HPM6750的外部Flash扇区划分与具体Flash芯片有关
+ * 这里以常见的4KB扇区为例
  */
-void V8M_McuFlashInit(void)
+static uint32_t get_sector_index(uint32_t address)
 {
-    //解锁
-    FLASH_Unlock();
-
-    //清除flash待处理标志位
-    FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR |
-                    FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
+    // 计算相对于Flash基址的偏移
+    uint32_t offset = address - FLASH_BASE_ADDR;
+    
+    // 扇区索引 = 偏移 / 扇区大小
+    return offset / FLASH_SECTOR_SIZE;
 }
 
 /**
- * @brief 从 mcu flash 读取 N 个字节
+ * @brief 擦除包含指定地址的扇区
  */
-int V8M_McuFlashReadNbytes(uint32_t ReadAddress, uint8_t *ReadBuf,
-                           int32_t ReadNum)
+static void flash_erase_sector(uint32_t address)
 {
-    int DataNum = 0;
-
-    while (DataNum < ReadNum)
-    {
-        *(ReadBuf + DataNum) = *(__IO uint8_t *)ReadAddress++;
-        DataNum++;
-    }
-
-    return DataNum;
+    flash_init();
+    
+    // HPM6750需要先禁用缓存，因为我们在擦除Flash
+    l1c_dc_invalidate_all();
+    
+    // 确保地址对齐到扇区边界
+    uint32_t sector_start = address & ~(FLASH_SECTOR_SIZE - 1);
+    
+    // 调用ROM API擦除扇区
+    rom_xpi_nor_erase_sector(HPM_XPI0, &s_flash_config, sector_start);
+    
+    // 擦除后清理缓存
+    l1c_dc_invalidate_all();
 }
 
 /**
- * @brief 往 mcu flash 指定地址写入 2 字节类型数据
+ * @brief 编程半字（2字节）到Flash
  */
-void V8M_McuFlashWriteHalfWord(uint32_t FlashAddress, uint16_t data)
+static void flash_program_halfword(uint32_t address, uint16_t data)
 {
-    while (FLASH_EraseSector(GetSector(FlashAddress), VoltageRange_3) !=
-           FLASH_COMPLETE)
-        ;
-    FLASH_ProgramHalfWord(FlashAddress, data);
+    flash_init();
+    
+    // 准备要写入的数据（需要是32位对齐）
+    uint32_t prog_data[2];
+    prog_data[0] = (uint32_t)data;  // 低16位有效
+    
+    // HPM6750要求编程地址对齐到编程单元
+    // 这里我们读取整个页，修改后再写回
+    uint8_t page_buffer[FLASH_PAGE_SIZE] __attribute__((aligned(4)));
+    
+    // 计算页起始地址
+    uint32_t page_start = address & ~(FLASH_PAGE_SIZE - 1);
+    uint32_t offset_in_page = address - page_start;
+    
+    // 先读取整个页
+    rom_xpi_nor_read(HPM_XPI0, &s_flash_config, page_start, page_buffer, FLASH_PAGE_SIZE);
+    
+    // 修改目标位置
+    *(uint16_t*)(page_buffer + offset_in_page) = data;
+    
+    // 擦除整个页所在的扇区
+    flash_erase_sector(address);
+    
+    // 重新编程整个页
+    rom_xpi_nor_program(HPM_XPI0, &s_flash_config, page_start, page_buffer, FLASH_PAGE_SIZE);
+    
+    // 清理缓存
+    l1c_dc_invalidate_all();
 }
 
 /**
- * @brief mcu flash 擦除页
- */
-void McuFlahErasePage() {}
-
-/**
- * @brief mcu flash 上锁，写入结束后执行
+ * @brief 从Flash读取N个字节
  */
-void V8M_McuFlashLock(void) { FLASH_Lock(); }
+int hpm_flash_read_nbytes(uint32_t read_addr, uint8_t *read_buf, int32_t read_num)
+{
+    flash_init();
+    
+    // HPM6750可以直接通过内存映射读取
+    // 但需要确保缓存一致性
+    l1c_dc_invalidate((uint32_t)read_addr, read_num);
+    
+    memcpy(read_buf, (void*)read_addr, read_num);
+    
+    return read_num;
+}
 
 /**
  * @brief 清除升级标志
  */
-void V8M_clear_iap_flag(void)
+void hpm_clear_iap_flag(void)
 {
-    __disable_irq();
-    V8M_McuFlashInit();
-    V8M_McuFlashWriteHalfWord(IAP_FLAG_ADDR, (uint16_t)0xFFFF);
-    V8M_McuFlashLock();
-    __enable_irq();
+   uint32_t mask;
+    
+    // 保存当前中断状态并关闭所有中断
+    mask = disable_global_irq(CSR_MSTATUS_MIE_MASK);
+    
+    // 写入0xFFFF表示无升级标志
+    flash_program_halfword(IAP_FLAG_ADDR, 0xFFFF);
+    
+     // 恢复中断状态
+    restore_global_irq(mask);
 }
 
 /**
  * @brief 记录升级标志
  */
-void V8M_record_iap_flag(void)
+void hpm_record_iap_flag(void)
 {
-    __disable_irq();
-    V8M_McuFlashInit();
-    V8M_McuFlashWriteHalfWord(IAP_FLAG_ADDR, IPA_FLAG_NEED_UPDATE);
-    V8M_McuFlashLock();
-    __enable_irq();
+    uint32_t mask;
+    
+    // 保存当前中断状态并关闭所有中断
+    mask = disable_global_irq(CSR_MSTATUS_MIE_MASK);
+    
+    flash_program_halfword(IAP_FLAG_ADDR, IAP_FLAG_NEED_UPDATE);
+    
+    // 恢复中断状态
+    restore_global_irq(mask);
 }
 
 /**
  * @brief 检查升级标志位
- * @note 主控的升级标志位位于 mcu 板载存储中，地址 0x800C000, 需要升级标志为
- * 0xABCD
  */
-void V8M_check_iap_flag(void)
+void hpm_check_iap_flag(void)
 {
-    uint16_t iapFlag;
-
-    V8M_McuFlashReadNbytes(IAP_FLAG_ADDR, (uint8_t *)&iapFlag, sizeof(iapFlag));
-
-    /* 如果升级标志位写的是需要升级，则写为不需要升级 */
-    if (iapFlag == IPA_FLAG_NEED_UPDATE)
-    {
-        V8M_clear_iap_flag();
+    uint16_t iap_flag;
+    
+    // 读取标志
+    hpm_flash_read_nbytes(IAP_FLAG_ADDR, (uint8_t*)&iap_flag, sizeof(iap_flag));
+    
+    // 如果需要升级，清除标志
+    if (iap_flag == IAP_FLAG_NEED_UPDATE) {
+        hpm_clear_iap_flag();
+        // 这里可以触发跳转到Bootloader的逻辑
+        // printf("IAP flag detected, jumping to bootloader...\n");
     }
 }
-
 #endif

controller_yy_app/v8/v8m_yy/bsp_V8M_YY_pwm.c

@@ -2,6 +2,8 @@
 #include "hpm_pwm_drv.h"
 // #include "hpm_trgm_drv.h"
 #include "bsp_V8M_YY_pwm.h"
+#include "hpm_gpio_drv.h"
+#include "hpm_gpiom_drv.h"
 #include "ver_config.h"
 #include "test.h"
 #include "main.h"
@@ -40,6 +42,12 @@
 #define PWM_MOTOR6_OUT           
 #define PWM_MOTOR6_CMP           5 
 
+#define PWM_EN_GPIO_CTRL     HPM_GPIO0
+#define PWM_EN_GPIO_INDEX    GPIO_DI_GPIOB
+#define PWM_EN_GPIO_PIN      3
+#define GPIOM_BASE           HPM_GPIOM
+#define GPIOM_PORT_ASSIGN    GPIOM_ASSIGN_GPIOB
+
 /*------------------ Macros definitions --------------------------------------*/
 
 #define V8M_MOTOR_PERIOD (2500*PRESCALER_FACTOR) /* PWM输出的周期，单位 us */
@@ -100,6 +108,7 @@ void Bsp_V8M_YY_PwmInit(void)
 {
     __motor_pwm_gpio_config();
     __motor_pwm_timer_config();
+
     // 输出默认值
     for (uint8_t i = V8M_PWM_YY_CH1; i <= V8M_PWM_YY_CH4; ++i)
         Bsp_V8M_YY_PwmSetCHValue(i, V8M_PWM_DEF_VALUE);
@@ -120,7 +129,12 @@ static void __motor_pwm_gpio_config(void)
     HPM_IOC->PAD[IOC_PAD_PB21].FUNC_CTL = IOC_PB21_FUNC_CTL_PWM1_P_3;
 
     HPM_IOC->PAD[IOC_PAD_PE06].FUNC_CTL = IOC_PE06_FUNC_CTL_PWM3_P_2;
-
+    
+    HPM_IOC->PAD[IOC_PAD_PB03].FUNC_CTL = IOC_PB03_FUNC_CTL_GPIO_B_03;
+    // PWM 使能引脚 失能后pwm信号无效 修改引脚后这里要修改  https://tools.hpmicro.com/pinmux
+    gpiom_set_pin_controller(GPIOM_BASE, GPIOM_PORT_ASSIGN, PWM_EN_GPIO_PIN, gpiom_soc_gpio0);
+    gpio_set_pin_output(PWM_EN_GPIO_CTRL, PWM_EN_GPIO_INDEX, PWM_EN_GPIO_PIN);
+    gpio_write_pin(PWM_EN_GPIO_CTRL, PWM_EN_GPIO_INDEX, PWM_EN_GPIO_PIN, 1);
    
 }
 
@@ -237,6 +251,18 @@ static void print_pwm_cmp_values_direct(void)
     
     printf("====================\n");
 }
+
+void PWM_IS_ENABLE(void)
+{
+   gpio_write_pin(PWM_EN_GPIO_CTRL, PWM_EN_GPIO_INDEX, PWM_EN_GPIO_PIN, 1);
+}
+
+void PWM_IS_DISABLE(void)
+{
+   gpio_write_pin(PWM_EN_GPIO_CTRL, PWM_EN_GPIO_INDEX, PWM_EN_GPIO_PIN, 0);
+}
+
+
 /* motor_pwm_test 经过测试 2026/03/13 */
 #ifdef PWM_TEST
 void v8m_yy_motor_pwm_test(void)

controller_yy_app/v8/v8m_yy/bsp_V8M_YY_pwm.h

@@ -20,4 +20,9 @@ void Bsp_V8M_YY_PwmSetCHValue(uint8_t chNum , uint32_t value);
 uint32_t Bsp_V8M_YY_PwmGetCHValue(uint8_t m_ch);
 
 void v8m_yy_motor_pwm_test(void);
+
+void PWM_IS_ENABLE(void);
+
+void PWM_IS_DISABLE(void);
+
 #endif

controller_yy_app/vklink/vklink.c

@@ -9,7 +9,7 @@
  */
 
 #include "vklink.h"
-#include "my_crc.h"
+#include <my_crc.h>
 #include <string.h>
 
 /**

controller_yy_app_controller_yy_board_flash_sdram_xip_debug/segger_embedded_studio/controlware_yy_app.emProject

@@ -74,12 +74,15 @@
       rv_toolchain_prefix="andes-"
       stack_size="0x4000"
       target_device_name="HPM6750xVMx" />
+    <configuration
+      Name="Debug"
+      build_output_file_name="$(OutDir)/controlware_yy$(EXE)" />
     <folder Name="application">
       <folder Name="controlware">
         <configuration
           Name="Common"
           build_intermediate_directory="Output/$(Configuration)/Obj/$(ProjectName)/application/controlware" />
-        <configuration Name="Debug" build_exclude_from_build="Yes" />
+        <configuration Name="Debug" build_exclude_from_build="No" />
         <file file_name="..\..\controller_yy_app\controlware\control_attitude.c">
           <configuration
             Name="Common"
@@ -110,7 +113,7 @@
         <configuration
           Name="Common"
           build_intermediate_directory="Output/$(Configuration)/Obj/$(ProjectName)/application/hardware" />
-        <configuration Name="Debug" build_exclude_from_build="Yes" />
+        <configuration Name="Debug" build_exclude_from_build="No" />
         <file file_name="..\..\controller_yy_app\hardware\hard_can.c">
           <configuration
             Name="Common"
@@ -155,16 +158,19 @@
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/hard_sdio_sd.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\hardware\hard_system.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/hard_system.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\hardware\hard_system_delay.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/hard_system_delay.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\hardware\hard_system_time.c">
           <configuration
@@ -181,7 +187,7 @@
         <configuration
           Name="Common"
           build_intermediate_directory="Output/$(Configuration)/Obj/$(ProjectName)/application/matrix" />
-        <configuration Name="Debug" build_exclude_from_build="Yes" />
+        <configuration Name="Debug" build_exclude_from_build="No" />
         <file file_name="..\..\controller_yy_app\matrix\euler.c">
           <configuration
             Name="Common"
@@ -302,7 +308,7 @@
         <configuration
           Name="Common"
           build_intermediate_directory="Output/$(Configuration)/Obj/$(ProjectName)/application/payload" />
-        <configuration Name="Debug" build_exclude_from_build="Yes" />
+        <configuration Name="Debug" build_exclude_from_build="No" />
         <file file_name="..\..\controller_yy_app\payload\payload.c">
           <configuration
             Name="Common"
@@ -313,7 +319,7 @@
         <configuration
           Name="Common"
           build_intermediate_directory="Output/$(Configuration)/Obj/$(ProjectName)/application/remote_controller" />
-        <configuration Name="Debug" build_exclude_from_build="Yes" />
+        <configuration Name="Debug" build_exclude_from_build="No" />
         <file file_name="..\..\controller_yy_app\remote_controller\rc_rock.c">
           <configuration
             Name="Common"
@@ -339,66 +345,78 @@
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/debug_printf.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\drv_uart.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/drv_uart.c$(OBJ)" />
         </file>
-        <file file_name="..\..\controller_yy_app\software\params.c">
+        <file file_name="params.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/params.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\rkfifo.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/rkfifo.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_can.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_can.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_can_yy.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_can_yy.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_delay.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_delay.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_flash.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_flash.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_gps.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_gps.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_gs.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_gs.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="Yes" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_imu.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_imu.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_motor_output.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_motor_output.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_payload.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_payload.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_port_uart4.c">
           <configuration
@@ -409,41 +427,49 @@
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_rc_input.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_sdcard.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_sdcard.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_system.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_system.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_time.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_time.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_timer.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_timer.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_usharprada.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_usharprada.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_voltage.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_voltage.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
         <file file_name="..\..\controller_yy_app\software\soft_warn.c">
           <configuration
             Name="Common"
             build_object_file_name="$(IntDir)/soft_warn.c$(OBJ)" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
         </file>
       </folder>
       <folder Name="user_src">
@@ -457,11 +483,12 @@
         </file>
       </folder>
       <folder Name="v8">
-        <configuration Name="Debug" build_exclude_from_build="Yes" />
+        <configuration Name="Debug" build_exclude_from_build="No" />
         <folder Name="v8m">
           <configuration
             Name="Common"
             build_intermediate_directory="Output/$(Configuration)/Obj/$(ProjectName)/application/v8/v8m" />
+          <configuration Name="Debug" build_exclude_from_build="No" />
           <file file_name="..\..\controller_yy_app\v8\v8m\bsp_V8M_adc.c">
             <configuration
               Name="Common"
@@ -513,7 +540,7 @@
         <configuration
           Name="Common"
           build_intermediate_directory="Output/$(Configuration)/Obj/$(ProjectName)/application/vklink" />
-        <configuration Name="Debug" build_exclude_from_build="Yes" />
+        <configuration Name="Debug" build_exclude_from_build="No" />
         <file file_name="..\..\controller_yy_app\vklink\gcs_vklink_v30.c">
           <configuration
             Name="Common"

controller_yy_app_controller_yy_board_flash_sdram_xip_debug/segger_embedded_studio/controlware_yy_app.emSession

@@ -22,7 +22,14 @@
   <ProjectSessionItem path="controlware_yy_app"/>
   <ProjectSessionItem path="controlware_yy_app;controlware_yy_app - controller_yy_board"/>
   <ProjectSessionItem path="controlware_yy_app;controlware_yy_app - controller_yy_board;application"/>
-  <ProjectSessionItem path="controlware_yy_app;controlware_yy_app - controller_yy_board;application;user_src"/>
+  <ProjectSessionItem path="controlware_yy_app;controlware_yy_app - controller_yy_board;application;controlware"/>
+  <ProjectSessionItem path="controlware_yy_app;controlware_yy_app - controller_yy_board;application;hardware"/>
+  <ProjectSessionItem path="controlware_yy_app;controlware_yy_app - controller_yy_board;application;software"/>
+  <ProjectSessionItem path="controlware_yy_app;controlware_yy_app - controller_yy_board;application;v8"/>
+  <ProjectSessionItem path="controlware_yy_app;controlware_yy_app - controller_yy_board;application;v8;v8m"/>
+  <ProjectSessionItem path="controlware_yy_app;controlware_yy_app - controller_yy_board;application;v8;v8m_yy"/>
+  <ProjectSessionItem path="controlware_yy_app;controlware_yy_app - controller_yy_board;boards"/>
+  <ProjectSessionItem path="controlware_yy_app;controlware_yy_app - controller_yy_board;boards;controller_yy_board"/>
  </Project>
  <Register1>
   <RegisterWindow invisibleNodes="" visibleNodes="ABI, RV32I/pc;ABI, RV32I/ra;ABI, RV32I/sp;ABI, RV32I/gp;ABI, RV32I/tp;ABI, RV32I/a0;ABI, RV32I/a1;ABI, RV32I/a2;ABI, RV32I/a3;ABI, RV32I/a4;ABI, RV32I/a5;ABI, RV32I/a6;ABI, RV32I/a7;ABI, RV32I/t0;ABI, RV32I/t1;ABI, RV32I/t2;ABI, RV32I/t3;ABI, RV32I/t4;ABI, RV32I/t5;ABI, RV32I/t6;ABI, RV32I/s0;ABI, RV32I/s1;ABI, RV32I/s2;ABI, RV32I/s3;ABI, RV32I/s4;ABI, RV32I/s5;ABI, RV32I/s6;ABI, RV32I/s7;ABI, RV32I/s8;ABI, RV32I/s9;ABI, RV32I/s10;ABI, RV32I/s11;CPU, RV32I/pc;CPU, RV32I/x1;CPU, RV32I/x2;CPU, RV32I/x3;CPU, RV32I/x4;CPU, RV32I/x5;CPU, RV32I/x6;CPU, RV32I/x7;CPU, RV32I/x8;CPU, RV32I/x9;CPU, RV32I/x10;CPU, RV32I/x11;CPU, RV32I/x12;CPU, RV32I/x13;CPU, RV32I/x14;CPU, RV32I/x15;CPU, RV32I/x16;CPU, RV32I/x17;CPU, RV32I/x18;CPU, RV32I/x19;CPU, RV32I/x20;CPU, RV32I/x21;CPU, RV32I/x22;CPU, RV32I/x23;CPU, RV32I/x24;CPU, RV32I/x25;CPU, RV32I/x26;CPU, RV32I/x27;CPU, RV32I/x28;CPU, RV32I/x29;CPU, RV32I/x30;CPU, RV32I/x31" binaryNodes="" asciiNodes="" openNodes="ABI, RV32I;CPU, RV32I" name="controlware_yy_app - controller_yy_board_Debug" decimalNodes="" octalNodes="" unsignedNodes=""/>
@@ -55,10 +62,8 @@
   <Watches active="0" update="Never"/>
  </Watch4>
  <Files>
-  <SessionOpenFile windowGroup="DockEditLeft" x="1" y="344" useTextEdit="1" path="../../controller_yy_app/middleware/hpm_math/sw_dsp/hpm_math_sw.c" left="0" top="326" codecName="Default"/>
-  <SessionOpenFile windowGroup="DockEditLeft" x="24" y="61" useTextEdit="1" path="../../controller_yy_app/user_src/main.c" left="0" selected="1" top="23" codecName="Default"/>
-  <SessionOpenFile windowGroup="DockEditLeft" x="0" y="0" useTextEdit="1" path="../../controller_yy_app/middleware/hpm_math/hpm_math.h" left="0" top="489" codecName="Default"/>
-  <SessionOpenFile windowGroup="DockEditLeft" x="9" y="4" useTextEdit="1" path="../../controller_yy_app/software/soft_inc/soft_gs.h" left="0" top="0" codecName="Default"/>
+  <SessionOpenFile windowGroup="DockEditLeft" x="5" y="12" useTextEdit="1" path="../../controller_yy_app/v8/v8m/bsp_V8M_flash.c" left="0" selected="1" top="0" codecName="Default"/>
+  <SessionOpenFile windowGroup="DockEditLeft" x="26" y="82" useTextEdit="1" path="../../controller_yy_board/board.c" left="0" top="63" codecName="Default"/>
  </Files>
  <EMStudioWindow activeProject="controlware_yy_app - controller_yy_board" fileDialogDefaultFilter="*.c" autoConnectTarget="GDB Server" buildConfiguration="Debug" sessionSettings="" debugSearchFileMap="" fileDialogInitialDirectory="" debugSearchPath="" autoConnectCapabilities="1343"/>
 </session>