基于tiva的匿名飞控学习笔记（1）
基于tiva的匿名飞控学习笔记（1）
- 开关状态任务
- 遥控器数据处理任务
- 数传数据交换
- 延时存储任务
开关状态任务
匿名飞控的开关状态任务为函数Swtich_State_Task(u8 dT_ms)，定义在Ano_FlightCtrl.c中，在任务调度中引用，1ms执行一次。主要负责确定一些传感器是否能用，如果正常就将相应的标志位置1（打开）。包括光流、激光、GPS(暂时没有)、UWB、OPENMV。
void Swtich_State_Task(u8 dT_ms)
{
switchs.baro_on = 1;
//光流模块
if(sens_hd_check.of_ok || sens_hd_check.of_df_ok)
{
//
if(sens_hd_check.of_ok)
{
jsdata.of_qua = OF_QUALITY;
jsdata.of_alt = (u16)OF_ALT;
}
else if(sens_hd_check.of_df_ok)
{
jsdata.of_qua = of_rdf.quality;
jsdata.of_alt = Laser_height_cm;
}
//
if(jsdata.of_qua>50 )//|| flag.flying == 0) //光流质量大于50 /*或者在飞行之前*/，认为光流可用，判定可用延迟时间为1秒
{
if(of_quality_delay<500)
{
of_quality_delay += dT_ms;
}
else
{
of_quality_ok = 1;
}
}
else
{
of_quality_delay =0;
of_quality_ok = 0;
}
//光流高度600cm内有效
if(jsdata.of_alt<600)
{
//
of_tof_on_tmp = 1;
jsdata.valid_of_alt_cm = jsdata.of_alt;
//延时1.5秒判断激光高度是否有效
if(of_alt_delay<1500)
{
of_alt_delay += dT_ms;
}
else
{
//判定高度有效
of_alt_ok = 1;
}
}
else
{
//
of_tof_on_tmp = 0;
//
if(of_alt_delay>0)
{
of_alt_delay -= dT_ms;
}
else
{
//判定高度无效
of_alt_ok = 0;
}
}
//
//
if(flag.flight_mode == LOC_HOLD)
{
if(of_alt_ok && of_quality_ok)
{
switchs.of_flow_on = 1;
}
else
{
switchs.of_flow_on = 0;
}
}
else
{
of_tof_on_tmp = 0;
switchs.of_flow_on = 0;
}
//
switchs.of_tof_on = of_tof_on_tmp;
}
else
{
switchs.of_flow_on = switchs.of_tof_on = 0;
}
//激光模块
if(sens_hd_check.tof_ok)
{
if(0)//(Laser_height_mm<1900)
{
switchs.tof_on = 1;
}
else
{
switchs.tof_on = 0;
}
}
else
{
switchs.tof_on = 0;
}
//GPS
//UWB
if(uwb_data.online && flag.flight_mode == LOC_HOLD)
{
switchs.uwb_on = 1;
}
else
{
switchs.uwb_on = 0;
}
//OPMV
if(opmv.offline==0 && flag.flight_mode == LOC_HOLD)
{
switchs.opmv_on = 1;
}
else
{
switchs.opmv_on = 0;
}
}
程序中的光流代码中对光流的有无、是否发生错误进行检查的，如果没有问题，就将sens_hd_check.of_ok || sens_hd_check.of_df_ok置为1，我觉得应该一个表示匿名光流，一个是优像光流。当得知光流可以使用后，在Swtich_State_Task中就可以将它打开了（同样是置标志位）。这个过程中加了一些延时，应该是出于安全考虑，也可能是光流初始化需要时间？然后需要对光流高度进行判断，如果小于6m。认为有效。只有以上工作全部成功，才能启用光流。否则，即使光流存在也只是个摆设。如果工作在定位模式，就将switchs.of_flow_on置为1。
激光模块是否正常通过标志sens_hd_check.tof_ok（在激光的初始化过程定义的）判断。下面的if(0)我感觉是没有用的，也就是说激光只要初始化成功就能用了。
UWB和openmv是只要判断了工作正常，模式正确就能打开。总体来说这个函数除了标志位还是标志位，没有什么特别的。应该算是一种保险措施。防止某一个模块突然不工作对无人机造成影响。
遥控器数据处理任务
遥控器数据处理任务函数RC_duty_task位于Ano_RC.c中，11ms执行一次。使用PPM协议或者SBUS协议，程序默认使用PPM协议。有关几种协议的介绍见链接: link
void RC_duty_task(u8 dT_ms) //建议2ms调用一次
{
if(flag.start_ok) //在主函数中被置为1
{
/////////////获得通道数据////////////////////////
if(RC_IN_MODE == PPM || RC_IN_MODE == PWM)
{
for(u8 i=0;i<CH_NUM;i++) //CH_NUM默认为8
{
if(chn_en_bit & (1<<i))//(Rc_Ppm_In[i]!=0)//该通道有值
{
//CH_N[]+1500为上位机显示通道值
CH_N[i] = ((s16)RC_PPM.Captures[i] - 1500); //1000 -- 2000us,处理成大约+-500摇杆量
}
else
{
CH_N[i] = 0;
}
CH_N[i] = LIMIT(CH_N[i],-500,500);//限制到+—500
}
}
else//sbus
{
for(u8 i=0;i<CH_NUM;i++)
{
if(chn_en_bit & (1<<i))//该通道有值
{
//CH_N[]+1500为上位机显示通道值
CH_N[i] = 0.65f *((s16)Rc_Sbus_In[i] - 1024); //248 --1024 --1800,处理成大约+-500摇杆量
}
else
{
CH_N[i] = 0;
}
CH_N[i] = LIMIT(CH_N[i],-500,500);//限制到+—500
}
}
///////////////////////////////////////////////
//解锁监测
unlock(dT_ms);
//摇杆触发功能监测
stick_function(dT_ms);
//通道看门狗
ch_watch_dog(dT_ms);
//失控保护检查
fail_safe_check(dT_ms);//3ms
}
}
RC_PPM.Captures[]和Rc_Sbus_In[i]都是对信号进行结算得到的值，相关函数位于Drv_RcIn.c中，计算过程是通用的，一般不需要修改，在调用
的时候可能需要将初始化部分修改一下（根据原理图修改相应的GPIO即可）。
整整一大段过程，PPM模式对我们工程有影响的数据只有RC_PPM.Captures[]和CH_N[i]，我们需要将数据发送到上位机显示以及通过接收的数据控制无人机。
unlock(dT_ms)为无人机上锁解锁函数，在电池电压、imu、气压计、惯性传感器工作正常的情况下才允许解锁，并且此时不能操作flash(自我感觉操作flash会降低运行速度，且一些参数正在被修改，如果此时解锁无法保证无人机正常飞行)。解锁方式是遥控器（美国手）遥杆内八或者外八，如果原本为上锁则解锁，原本为解锁状态则上锁（在操作飞控过程中切记不要把遥杆打到内八或外八）。建议这部分程序不要更改！
stick_function(dT_ms)为遥杆触发功能检测，也就是根据相应的通道值判断无人机俯仰、滚转或者偏航，然后校准相应的传感器（陀螺仪，加速度计，磁力计），注意这个函数不是用来控制无人机状态的，只是根据无人机状态校准传感器的。
fail_safe_check(dT_ms)为失控保护检查，当通道没有信号或者接收到失控保护信号时就启动失控保护，如果有GPS就返航，无GPS直接降落。当然并不是一次异常就判断为失控，需要多次不能接受信号或收到失控保护信号在进行失控保护。
数传数据交换
数传数据交换任务ANO_DT_Data_Exchange()定义在Ano_RC.c中，在我看来就是加了协议的串口通信。数据传出协议与匿名上位机相对应，可以查看匿名上位机的手册编写这部分函数。不过有些函数是固定的，比如pid参数，飞控状态等，这些直接和匿名上位机相对应，建议不要更改，如果要传输自己添加的某些数据，可选用其他通道，避免和默认的数据传输冲突，如何编写数据传输程序可以查看匿名的上位机教程 link.
void ANO_DT_Data_Exchange(void)
{
static u16 cnt = 0;
static u16 senser_cnt = 10;
static u16 senser2_cnt = 50;
static u16 user_cnt = 10;
static u16 status_cnt = 15;
static u16 rcdata_cnt = 20;
static u16 motopwm_cnt = 20;
static u16 power_cnt = 50;
static u16 speed_cnt = 50;
static u16 sensorsta_cnt = 500;
static u16 omv_cnt = 100;
static u16 location_cnt = 500;
static u8 flag_send_omv = 0;
if((cnt % senser_cnt) == (senser_cnt-1))
f.send_senser = 1;
if((cnt % senser2_cnt) == (senser2_cnt-1))
f.send_senser2 = 1;
if((cnt % user_cnt) == (user_cnt-2))
f.send_user = 1;
if((cnt % status_cnt) == (status_cnt-1))
f.send_status = 1;
if((cnt % rcdata_cnt) == (rcdata_cnt-1))
f.send_rcdata = 1;
if((cnt % motopwm_cnt) == (motopwm_cnt-2))
f.send_motopwm = 1;
if((cnt % power_cnt) == (power_cnt-2))
f.send_power = 1;
if((cnt % speed_cnt) == (speed_cnt-3))
f.send_speed = 1;
if((cnt % sensorsta_cnt) == (sensorsta_cnt-2))
{
f.send_sensorsta = 1;
}
if((cnt % omv_cnt) == (omv_cnt-2))
{
flag_send_omv = 1;
}
if((cnt % location_cnt) == (location_cnt-3))
{
f.send_location = 1;
}
if(++cnt>1000) cnt = 0;
/////////////////////////////////////////////////////////////////////////////////////
if(f.send_version)
{
f.send_version = 0;
ANO_DT_Send_Version(4,300,100,400,0); //发送版本
}
else if(f.paraToSend < 0xffff)
{
ANO_DT_SendParame(f.paraToSend);
f.paraToSend = 0xffff;
}
///////////////////////////////////////////////////////////////////////////////////////
else if(f.send_status)
{
f.send_status = 0;
ANO_DT_Send_Status(imu_data.rol,imu_data.pit,imu_data.yaw,wcz_hei_fus.out,(flag.flight_mode+1),flag.unlock_sta);
}
///////////////////////////////////////////////////////////////////////////////////////
else if(f.send_speed)
{
f.send_speed = 0;
ANO_DT_Send_Speed(loc_ctrl_1.fb[Y],loc_ctrl_1.fb[X],loc_ctrl_1.fb[Z]);
}
///////////////////////////////////////////////////////////////////////////////////////
else if(f.send_user)
{
f.send_user = 0;
ANO_DT_Send_User();
}
///////////////////////////////////////////////////////////////////////////////////////
else if(f.send_senser)
{
f.send_senser = 0;
ANO_DT_Send_Senser(sensor.Acc[X],sensor.Acc[Y],sensor.Acc[Z],sensor.Gyro[X],sensor.Gyro[Y],sensor.Gyro[Z],mag.val[X],mag.val[Y],mag.val[Z]);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_senser2)
{
f.send_senser2 = 0;
ANO_DT_Send_Senser2(baro_height,ref_tof_height,sensor.Tempreature_C*10);//原始数据
}
/////////////////////////////////////////////////////////////////////////////////////
else if(flag_send_omv)
{
flag_send_omv = 0;
if(f.send_omv_ct)
{
f.send_omv_ct = 0;
ANO_DT_SendOmvCt(opmv.cb.color_flag,opmv.cb.sta,opmv.cb.pos_x,opmv.cb.pos_y,opmv.cb.dT_ms);
}
else if(f.send_omv_lt)
{
f.send_omv_lt = 0;
ANO_DT_SendOmvLt(opmv.lt.sta, opmv.lt.angle, opmv.lt.deviation, opmv.lt.p_flag, opmv.lt.pos_x, opmv.lt.pos_y, opmv.lt.dT_ms);
}
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_rcdata)
{
f.send_rcdata = 0;
s16 CH_GCS[CH_NUM];
for(u8 i=0;i<CH_NUM;i++)
{
if((chn_en_bit & (1<<i)))//(Rc_Pwm_In[i]!=0 || Rc_Ppm_In[i] !=0 )//该通道有值
{
CH_GCS[i] = CH_N[i] + 1500;
}
else
{
CH_GCS[i] = 0;
}
}
ANO_DT_Send_RCData(CH_GCS[2],CH_GCS[3],CH_GCS[0],CH_GCS[1],CH_GCS[4],CH_GCS[5],CH_GCS[6],CH_GCS[7],0,0);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_motopwm)
{
f.send_motopwm = 0;
#if MOTORSNUM == 8
ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],motor[4],motor[5],motor[6],motor[7]);
#elif MOTORSNUM == 6
ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],motor[4],motor[5],0,0);
#elif MOTORSNUM == 4
ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],0,0,0,0);
#else
#endif
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_power)
{
f.send_power = 0;
ANO_DT_Send_Power(Plane_Votage*100,0);
}
else if(f.send_sensorsta)
{
f.send_sensorsta = 0;
ANO_DT_SendSensorSta(switchs.of_flow_on ,switchs.gps_on,switchs.opmv_on,switchs.uwb_on,switchs.of_tof_on);
}
else if(f.send_location)
{
f.send_location = 0;
ANO_DT_Send_Location(switchs.gps_on,Gps_information.satellite_num,(s32)Gps_information.longitude,(s32)Gps_information.latitude,123,456);
}
else if(f.send_vef)
{
ANO_DT_Send_VER();
f.send_vef = 0;
}
/////////////////
ANO_DT_Data_Receive_Anl_Task();
////////////////////////////////////////////////////////////////////
}
前半部分主要是用来对任务进行分配，这么多组数据一起发送肯定乱套了。然后就是根据分配的任务依次执行了。
ANO_DT_Send_Version应该是用来发送版本信息的
ANO_DT_SendParame用来发送参数信息（pid等）
ANO_DT_Send_Status用来发送飞控状态（俯仰、偏航角等）
ANO_DT_Send_Speed用来发送x、y、z方向的速度
ANO_DT_Send_User这个就是我刚才说的自定义数据发送，如果用到的话可以查看上位机视频或手册。
ANO_DT_Send_Senser用来发送陀螺仪、加速度计、磁力计的数据
ANO_DT_Send_Senser2用来发送气压计、激光数据以及传感器温度
ANO_DT_SendOmvCt、ANO_DT_SendOmvCt用来发送openmv数据
ANO_DT_Send_RCData用来发送遥控器数据信息（处理之后）
ANO_DT_Send_MotoPWM用来发送控制电机的pwm信息
ANO_DT_Send_Power用来发送电源电压
ANO_DT_SendSensorSta用来发送传感器状态（有无或是否正常工作）
ANO_DT_Send_Location用来发送位置信息，需要GPS
ANO_DT_Data_Receive_Anl_Task是数据接收任务，首先在串口中断中ANO_DT_Data_Receive_Prepare(com_data)接收数据并存放在DT_RxBuffer中，由ANO_DT_Data_Receive_Anl根据得到的命令来控制飞控（恢复默认参数、改写参数、控制飞行等）
延时存储任务
延时存储任务Ano_Parame_Write_task()50ms执行一次.需要注意的是参数改变是不会立刻存入flash，因为为写入flash耗时较长，所以当无人机工作时如果需要存储数据，会置一个标志位，等到无人机降落锁定后在存入flash，保证飞行的安全性。代码如下：
void Ano_Parame_Write_task(u16 dT_ms)
{
//因为写入flash耗时较长，我们飞控做了一个特殊逻辑，在解锁后，是不进行参数写入的，此时会置一个需要写入标志位，等飞机降落锁定后，再写入参数，提升飞行安全性
//为了避免连续更新两个参数，造成flash写入两次，我们飞控加入一个延时逻辑，参数改变后三秒，才进行写入操作，可以一次写入多项参数，降低flash擦写次数
if(para_sta.save_en ) //允许存储
{
if(para_sta.save_trig == 1) //如果触发存储标记1
{
LED_STA.saving = 1;
para_sta.time_delay = 0; //计时复位
para_sta.save_trig = 2; //触发存储标记2
}
if(para_sta.save_trig == 2) //如果触发存储标记2
{
if(para_sta.time_delay<3000) //计时小于3000ms
{
para_sta.time_delay += dT_ms; //计时
}
else
{
para_sta.save_trig = 0; //存储标记复位
Ano_Parame_Write(); //执行存储
ANO_DT_SendString("Set save OK!");
LED_STA.saving = 0;
}
}
else
{
para_sta.time_delay = 0;
}
}
else
{
para_sta.time_delay = 0;
para_sta.save_trig = 0;
}
}
但这样就会出现一个问题，在我们调飞控的过程中改变参数，无人机不会立刻做出响应？其实不会的。我们更改参数有两种途径，一种是通过上位机修改，当我们修改完之后会调用ANO_DT_ParListToParUsed()函数，直接将参数改过来。另一种是在软件上修改PID_Rest()和Parame_Reset()中的参数，在我们上电后程序会先进一个初始化//数据初始化 Dvr_ParamterInit(); //读取初始数据 Para_Data_Init();
如果我们是第一次烧写程序，那么读取数据过程就会将PID_Rest()和Parame_Reset()的数据直接存入内存。但如果我们之前已经下载过程序的话，那么Ano_Parame.set.frist_init = SOFT_VER就会被存入内存中，这个时候读数据过程就不会重新写入数据了。代码如下：
static void Ano_Parame_Write(void)
{
All_PID_Init(); //////存储PID参数后，重新初始化PID
Ano_Parame.set.frist_init = SOFT_VER;
Parame_Copy_Fc2para();
Dvr_ParamterSave();
}
void Ano_Parame_Read(void)
{
Dvr_ParamterRead();
if(Ano_Parame.set.frist_init != SOFT_VER) //内容没有被初始化，则进行参数初始化工作
{
Parame_Reset();
PID_Rest();
Ano_Parame_Write();
}
Parame_Copy_Para2fc();
}
这个时候我们可以通过上位机恢复默认参数将PID_Rest()和Parame_Reset()的数据存入内存。
默认保存的数据如下
__packed struct Parameter_s
{
u16 frist_init; //飞控第一次初始化，需要做一些特殊工作，比如清空flash
u8 pwmInMode; //接收机模式，分别为PWM型PPM型
u8 heatSwitch; //
float acc_offset[VEC_XYZ]; //加速度计零偏
float gyro_offset[VEC_XYZ]; //陀螺仪零偏
float surface_vec[VEC_XYZ]; //水平面向量
float center_pos_cm[VEC_XYZ]; //重心相对传感器位置偏移量
float mag_offset[VEC_XYZ]; //磁力计零偏
float mag_gain[VEC_XYZ]; //磁力计校正比例
float pid_att_1level[VEC_RPY][PID]; //姿态控制角速度环PID参数
float pid_att_2level[VEC_RPY][PID]; //姿态控制角度环PID参数
float pid_alt_1level[PID]; //高度控制高度速度环PID参数
float pid_alt_2level[PID]; //高度控制高度环PID参数
float pid_loc_1level[PID]; //位置控制位置速度环PID参数
float pid_loc_2level[PID]; //位置控制位置环PID参数
float pid_gps_loc_1level[PID]; //位置控制位置速度环PID参数
float pid_gps_loc_2level[PID]; //位置控制位置环PID参数
float warn_power_voltage;
float return_home_power_voltage;
float lowest_power_voltage;
float auto_take_off_height;
float auto_take_off_speed;
float auto_landing_speed;
float idle_speed_pwm;
};
直接存入内存的参数名称如下，上位机得到的参数数据也会赋值给下列参数控制系统。
//--- 姿态控制角速度环PID参数
Ano_Parame.set.pid_att_1level[ROL][KP] = 4.0f; //姿态控制角速度环PID参数
Ano_Parame.set.pid_att_1level[ROL][KI] = 3.0f; //姿态控制角速度环PID参数
Ano_Parame.set.pid_att_1level[ROL][KD] = 0.15f; //姿态控制角速度环PID参数
Ano_Parame.set.pid_att_1level[PIT][KP] = 4.0f; //姿态控制角速度环PID参数
Ano_Parame.set.pid_att_1level[PIT][KI] = 3.0f; //姿态控制角速度环PID参数
Ano_Parame.set.pid_att_1level[PIT][KD] = 0.15f; //姿态控制角速度环PID参数
Ano_Parame.set.pid_att_1level[YAW][KP] = 6.0f; //姿态控制角速度环PID参数
Ano_Parame.set.pid_att_1level[YAW][KI] = 0.5f; //姿态控制角速度环PID参数
Ano_Parame.set.pid_att_1level[YAW][KD] = 0.0f; //姿态控制角速度环PID参数
//--- 姿态控制角度环PID参数
Ano_Parame.set.pid_att_2level[ROL][KP] = 7.0f; //姿态控制角度环PID参数
Ano_Parame.set.pid_att_2level[ROL][KI] = 0.0f; //姿态控制角度环PID参数
Ano_Parame.set.pid_att_2level[ROL][KD] = 0.00f; //姿态控制角度环PID参数
Ano_Parame.set.pid_att_2level[PIT][KP] = 7.0f; //姿态控制角度环PID参数
Ano_Parame.set.pid_att_2level[PIT][KI] = 0.0f; //姿态控制角度环PID参数
Ano_Parame.set.pid_att_2level[PIT][KD] = 0.00f; //姿态控制角度环PID参数
Ano_Parame.set.pid_att_2level[YAW][KP] = 5.0f; //姿态控制角度环PID参数
Ano_Parame.set.pid_att_2level[YAW][KI] = 0.0f; //姿态控制角度环PID参数
Ano_Parame.set.pid_att_2level[YAW][KD] = 0.5; //姿态控制角度环PID参数
//--- 高度控制高度速度环PID参数
Ano_Parame.set.pid_alt_1level[KP] = 2.0f; //高度控制高度速度环PID参数
Ano_Parame.set.pid_alt_1level[KI] = 1.0f; //高度控制高度速度环PID参数
Ano_Parame.set.pid_alt_1level[KD] = 0.05f; //高度控制高度速度环PID参数
//--- 高度控制高度环PID参数
Ano_Parame.set.pid_alt_2level[KP] = 1.0f; //高度控制高度环PID参数
Ano_Parame.set.pid_alt_2level[KI] = 0; //高度控制高度环PID参数(NULL)
Ano_Parame.set.pid_alt_2level[KD] = 0; //高度控制高度环PID参数(NULL)
//--- 位置控制位置速度环PID参数
Ano_Parame.set.pid_loc_1level[KP] = 0.15f; //位置控制位置速度环PID参数
Ano_Parame.set.pid_loc_1level[KI] = 0.10f; //位置控制位置速度环PID参数
Ano_Parame.set.pid_loc_1level[KD] = 0.00f; //位置控制位置速度环PID参数
//--- 位置控制位置环PID参数
Ano_Parame.set.pid_loc_2level[KP] = 0; //位置控制位置环PID参数(NULL)
Ano_Parame.set.pid_loc_2level[KI] = 0; //位置控制位置环PID参数(NULL)
Ano_Parame.set.pid_loc_2level[KD] = 0; //位置控制位置环PID参数(NULL)
//--- GPS位置控制位置速度环PID参数
Ano_Parame.set.pid_gps_loc_1level[KP] = 0.15f; //位置控制位置速度环PID参数
Ano_Parame.set.pid_gps_loc_1level[KI] = 0.10f; //位置控制位置速度环PID参数
Ano_Parame.set.pid_gps_loc_1level[KD] = 0.00f; //位置控制位置速度环PID参数
//--- GPS位置控制位置环PID参数
Ano_Parame.set.pid_gps_loc_2level[KP] = 0.3f; //位置控制位置环PID参数
Ano_Parame.set.pid_gps_loc_2level[KI] = 0; //位置控制位置环PID参数(NULL)
Ano_Parame.set.pid_gps_loc_2level[KD] = 0; //位置控制位置环PID参数(NULL)
Ano_Parame.set.pwmInMode = PPM;
Ano_Parame.set.heatSwitch = 0;
Ano_Parame.set.warn_power_voltage = 3.50f *3;
Ano_Parame.set.return_home_power_voltage = 3.7f *3;
Ano_Parame.set.lowest_power_voltage = 3.4f *3;
Ano_Parame.set.auto_take_off_height = 0;//cm
Ano_Parame.set.auto_take_off_speed = 150;
Ano_Parame.set.auto_landing_speed = 60;
Ano_Parame.set.idle_speed_pwm = 20;//20%
for(u8 i = 0;i<3;i++)
{
Ano_Parame.set.acc_offset[i] = 0;
Ano_Parame.set.gyro_offset[i] = 0;
Ano_Parame.set.mag_offset[i] = 0;
Ano_Parame.set.mag_gain[i] = 1;
Ano_Parame.set.center_pos_cm[i] = 0;
}
//等号前面为存入内存的数据，等号后面为飞控使用的参数名称
Ano_Parame.set.acc_offset[i] = save.acc_offset[i];
Ano_Parame.set.gyro_offset[i] = save.gyro_offset[i];
Ano_Parame.set.mag_offset[i] = save.mag_offset[i];
Ano_Parame.set.mag_gain[i] = save.mag_gain[i];