#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 #include #include // 定义用来存储数据的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); }