无名飞控的姿态解算和控制（四）

最新推荐文章于 2022-05-08 08:21:00 发布
最新推荐文章于 2022-05-08 08:21:00 发布
阅读量2k 收藏 12
点赞数 1
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/wang2012113132/article/details/78932345
版权

上面几篇帖子已经写完控制流程还剩一点没说

 if(PPM_Isr_Cnt==100)//PPM接收正常才进行传感器标定检测
    {
        Accel_Calibration_Check();//加速度标定检测
        Mag_Calibration_Check();//磁力计标定检测
    }
    Bling_Working(Bling_Mode);//状态指示灯
    //DMA_Send_StateMachine();//DMA传输，只使用山外上位机
    //ANO_SEND_StateMachine();//DMA传输，只使用ANO地面站
    DMA_SEND_Tradeoff();//DMA传输，两路地面站机同时工作

    TIM_ClearITPendingBit(TIM4,TIM_FLAG_Update);
 }
对于变量PPM_Isr_Cnt，它是一个全局变量uint16 PPM_Isr_Cnt=0;

PPM的中断函数

uint16 PPM_Sample_Cnt=0;
uint16 PPM_Isr_Cnt=0;
u32 Last_PPM_Time=0;
u32 PPM_Time=0;
u16 PPM_Time_Delta=0;
u16 PPM_Time_Max=0;
uint16 PPM_Start_Time=0;
uint16 PPM_Finished_Time=0;
uint16 PPM_Is_Okay=0;
uint16 PPM_Databuf[8]={0};

/***************************************************
函数名: void EXTI9_5_IRQHandler(void)
说明:	PPM接收中断函数
入口:	无
出口:	无
备注:	程序初始化后、始终运行
****************************************************/
void EXTI9_5_IRQHandler(void)
{
  if(EXTI_GetITStatus(EXTI_Line8) != RESET)
  {
//系统运行时间获取，单位us
    Last_PPM_Time=PPM_Time;
    PPM_Time=10000*TIME_ISR_CNT
                   +TIM2->CNT/2;//us
    PPM_Time_Delta=PPM_Time-Last_PPM_Time;
    //PPM中断进入判断
    if(PPM_Isr_Cnt<100)  PPM_Isr_Cnt++;
   //PPM解析开始
    if(PPM_Is_Okay==1)
    {
    PPM_Sample_Cnt++;
    //对应通道写入缓冲区
    PPM_Databuf[PPM_Sample_Cnt-1]=PPM_Time_Delta;
    //单次解析结束
      if(PPM_Sample_Cnt>=8)
        PPM_Is_Okay=0;
    }
    if(PPM_Time_Delta>=2050)//帧结束电平至少2ms=2000us
    {
      PPM_Is_Okay=1;
      PPM_Sample_Cnt=0;
    }
  }
  EXTI_ClearITPendingBit(EXTI_Line8);
}
PPM_Isr_Cnt在小于100时，每次中断发生自加一次，直到等于100；

然后

Accel_Calibration_Check();//加速度标定检测
 Mag_Calibration_Check();//磁力计标定检测
对于加速度 

uint8_t flight_direction=6;
uint8_t Accel_Calibration_Flag=0;//加速度计校准模式
uint8_t Accel_Calibration_Finished[6]={0,0,0,0,0,0};//对应面校准完成标志位
uint8_t Accel_Calibration_All_Finished=0;//6面校准全部校准完成标志位
uint16_t Accel_Calibration_Makesure_Cnt=0;
uint16_t Accel_flight_direction_cnt=0;
void Accel_Calibration_Check(void)
{
   uint16_t  i=0;
   if(Throttle_Control==1000&&Yaw_Control>=80&&Roll_Control<=-40&&Pitch_Control>=40)
   {
      Accel_Calibration_Makesure_Cnt++;
   }
   if(Throttle_Control==1000
      &&Yaw_Control>=80
        &&Roll_Control<=-40
          &&Pitch_Control>=40
            &&Accel_Calibration_Makesure_Cnt>=200*3)//持续三秒
  {
      Bling_Mode=1;
      Accel_Calibration_Flag=1;//加速度校准模式
      Cal_Flag=0;
      Bling_Set(&Light_1,1000,100,0.5,0,GPIOC,GPIO_Pin_4,1);
      Bling_Set(&Light_2,1000,100,0.5,0,GPIOC,GPIO_Pin_5,1);
      Bling_Set(&Light_3,1000,100,0.5,0,GPIOC,GPIO_Pin_10,1);
      flight_direction=6;
      Accel_Calibration_All_Finished=0;//全部校准完成标志位清零
      Accel_Calibration_Makesure_Cnt=0;
      for(i=0;i<6;i++)
      {
        Accel_Calibration_Finished[i]=0;//对应面标志位清零
        acce_sample[i].x=0; //清空对应面的加速度计量
        acce_sample[i].y=0; //清空对应面的加速度计量
        acce_sample[i].z=0; //清空对应面的加速度计量
      }
      Page_Number=10;//OLED加速度计矫正页面
      Reset_Mag_Calibartion(1);
  }

  if(Accel_Calibration_Flag==1)
  {
     if(Throttle_Control==1000&&Yaw_Control<=-80&&Roll_Control==0&&Pitch_Control==0)
     {
       Accel_flight_direction_cnt++;
       if(Accel_flight_direction_cnt>=4*25)//100ms
         flight_direction=0;

     }
     else if(Throttle_Control==1000&&Yaw_Control==0&&Roll_Control>=40&&Pitch_Control==0)
     {
       Accel_flight_direction_cnt++;
       if(Accel_flight_direction_cnt>=4*25)//100ms
         flight_direction=1;
     }
     else if(Throttle_Control==1000&&Yaw_Control==0&&Roll_Control<=-40&&Pitch_Control==0)
     {
       Accel_flight_direction_cnt++;
       if(Accel_flight_direction_cnt>=4*25)//100ms
         flight_direction=2;
     }
     else if(Throttle_Control==1000&&Yaw_Control==0&&Roll_Control==0&&Pitch_Control>=40)
     {
       Accel_flight_direction_cnt++;
       if(Accel_flight_direction_cnt>=4*25)//100ms
         flight_direction=3;
     }
     else if(Throttle_Control==1000&&Yaw_Control==0&&Roll_Control==0&&Pitch_Control<=-40)
     {
       Accel_flight_direction_cnt++;
       if(Accel_flight_direction_cnt>=4*25)//100ms
         flight_direction=4;
     }
     else if(Throttle_Control==1000&&Yaw_Control>80&&Roll_Control==0&&Pitch_Control==0)
     {
       Accel_flight_direction_cnt++;
       if(Accel_flight_direction_cnt>=4*25)//100ms
       flight_direction=5;
     }
     else
     {
       Accel_flight_direction_cnt/=2;
     }

   if(Accel_flight_direction_cnt>=200)  Accel_flight_direction_cnt=0;

 }

}
加速度检测，每次
这个过程可以分为两部分，一部分是根据遥控器摇杆确认加速度是否需要校准，如需要收集数据，另外引入磁力计校准，另一部分根据摇杆确认flight_direction的值，

校准就是把所有的数据清零？？

其中Reset_Mag_Calibartion函数

void Reset_Mag_Calibartion(uint8_t Type)
{
  uint16 i=0;
  for(i=0;i<12;i++)
  {
    Mag_360_Flag[0][i]=0;//清空采集角点
    Mag_360_Flag[1][i]=0;//清空采集角点
  }
  Mag_Is_Okay_Flag[0]=0;
  Mag_Is_Okay_Flag[1]=0;
  Mag_Calibration_Mode=2;
  if(Type==1)  Mag_Calibration_Flag=0;
}

对于Mag_Calibration_Check 


/***********磁力计中心矫正，取单轴最大、最小值平均******/
uint8_t Mag_Calibration_Flag=0,Mag_Calibration_All_Finished;
uint8_t Mag_Calibration_Finished[2]={0};
uint16_t Mag_Calibration_Makesure_Cnt=0;
uint8_t  Mag_Calibration_Mode=2;
uint16_t Mag_Calibration_Cnt=0;
/*********************************************/
const int16_t Mag_360_define[12]={
0,30,60,90,
120,150,180,210,
240,270,300,330
};//磁力计矫正遍历角度，确保数据采集充分
uint8_t Mag_360_Flag[2][12]={0};
uint16_t Mag_Is_Okay_Flag[2];
Calibration Mag;
Mag_Unit DataMag;
Mag_Unit Mag_Offset_Read={
  0,0,0,
};
void Mag_Calibration_Check(void)
{
   uint16_t  i=0,j=0;
   if(Throttle_Control==1000&&Yaw_Control>=80&&Roll_Control>=40&&Pitch_Control>=40) Mag_Calibration_Makesure_Cnt++;
   if(Throttle_Control==1000
      &&Yaw_Control>=80
        &&Roll_Control>=40
          &&Pitch_Control>=40
           &&Mag_Calibration_Makesure_Cnt>250*4//持续4S
            )//进入磁力计校准模式
  {
      Bling_Mode=2;
      Mag_Calibration_Flag=1;//磁力计校准模式
      Mag_Calibration_Mode=2;
      Bling_Set(&Light_1,1000,500,0.2,0,GPIOC,GPIO_Pin_4,1);
      Bling_Set(&Light_2,1000,500,0.5,0,GPIOC,GPIO_Pin_5,1);
      Bling_Set(&Light_3,1000,500,0.7,0,GPIOC,GPIO_Pin_10,1);
      Mag_Calibration_Makesure_Cnt=0;
      Mag_Calibration_All_Finished=0;//全部校准完成标志位清零
      for(i=0;i<2;i++)
      {
        Mag_Calibration_Finished[i]=0;//对应面标志位清零
        for(j=0;j<12;j++) {Mag_360_Flag[i][j]=0;}
      }
      Page_Number=11;
      Reset_Accel_Calibartion(1);
  }

  if(Mag_Calibration_Flag==1)
  {
     if(Throttle_Control==1000
        &&Yaw_Control<=-80
          &&Roll_Control==0
            &&Pitch_Control==0) //第一面矫正
     {
         Mag_Calibration_Cnt++;
         if(Mag_Calibration_Cnt>=4*25)
         {
            Mag_Calibration_Mode=0;
            Mag_Is_Okay_Flag[1]=0;//单面数据采集完成标志位置0
            for(i=0;i<12;i++) Mag_360_Flag[1][i]=0;//清空采集角遍历数据点
         }
     }
  else if(Throttle_Control==1000
             &&Yaw_Control>80
               &&Roll_Control==0
                 &&Pitch_Control==0) //第二面矫正
     {
         Mag_Calibration_Cnt++;
         if(Mag_Calibration_Cnt>=4*25)
         {
             Mag_Calibration_Mode=1;
             Mag_Is_Okay_Flag[1]=0;//单面数据采集完成标志位置0
             for(i=0;i<12;i++) Mag_360_Flag[1][i]=0;//清空采集角遍历数据点
         }
     }
  else
  {
    Mag_Calibration_Cnt/=2;
  }
  if(Mag_Calibration_Cnt>=200)  Mag_Calibration_Cnt=200;

  }

}
不懂原理唉，不知道为什么？查查在完整吧！ 


Bling_Working(Bling_Mode);//状态指示灯
    //DMA_Send_StateMachine();//DMA传输，只使用山外上位机
    //ANO_SEND_StateMachine();//DMA传输，只使用ANO地面站
    DMA_SEND_Tradeoff();//DMA传输，两路地面站机同时工作
从这里看飞控可以选择DMA传输，先从一个开始吧！

这里用的是

uint16_t DMA_SEND_CNT=0;
//由于共用同一个DMA，不能同时发否则会有一个丢帧
void DMA_SEND_Tradeoff(void)
{
     DMA_SEND_CNT++;
     if(DMA_SEND_CNT<=16&&DMA_SEND_CNT>=4)
     {
         DMA_Send_StateMachine();//DMA传输
     }
     else if(DMA_SEND_CNT==20)//100ms进入一次
     {
         ANO_SEND_StateMachine();
         DMA_SEND_CNT=0;
     }
}

通过DMA_SEND_CNT来判断进入那一个函数即地面站

先追DMA_Send_StateMachine()

DMA_Vcan_Buff  Vcan_Buff;
#define DMA_SEND_PERIOD 3//4*5=20ms,周期太小不易于观察波形
void DMA_Send_StateMachine(void)
{
  static uint16_t DMA_Send_Cnt=0;
  DMA_Send_Cnt++;
  if(DMA_Send_Cnt>=DMA_SEND_PERIOD)
  {
   Vcan_Buff.Head=0xfc030000;
/*
  Vcan_Buff.DataBuf[0]=Pitch;
  Vcan_Buff.DataBuf[1]=Roll;
  Vcan_Buff.DataBuf[2]=ACCE_X;
  Vcan_Buff.DataBuf[3]=ACCE_Y;
  Vcan_Buff.DataBuf[4]=Origion_NamelessQuad.Acceleration[_YAW];
  Vcan_Buff.DataBuf[5]=Filter_Feedback_NamelessQuad.Acceleration[_YAW];
  //Vcan_Buff.DataBuf[6]=Total_Controler.High_Position_Control.Expect;
  //Vcan_Buff.DataBuf[7]=FilterBefore_NamelessQuad.Acceleration[_YAW];
  Vcan_Buff.DataBuf[7]=Gyro_Delta_Length;
  Vcan_Buff.DataBuf[6]=Acceleration_Length;
*/
  Vcan_Buff.DataBuf[0]=NamelessQuad.Position[_YAW];
  Vcan_Buff.DataBuf[1]=NamelessQuad.Speed[_YAW];
  Vcan_Buff.DataBuf[2]=NamelessQuad.Acceleration[_YAW];
  Vcan_Buff.DataBuf[3]=Altitude_Estimate;
  Vcan_Buff.DataBuf[4]=Origion_NamelessQuad.Acceleration[_YAW];
  Vcan_Buff.DataBuf[5]=acc_correction[_YAW];
  Vcan_Buff.DataBuf[7]=vel_correction[_YAW];
  Vcan_Buff.DataBuf[6]=pos_correction[_YAW];


/*
  Vcan_Buff.DataBuf[0]=NamelessQuad.Position[_PITCH];
  Vcan_Buff.DataBuf[1]=NamelessQuad.Speed[_PITCH];
  Vcan_Buff.DataBuf[2]=GPS_Vel.E;
  Vcan_Buff.DataBuf[3]=Earth_Frame_To_XYZ.E;

  Vcan_Buff.DataBuf[4]=NamelessQuad.Position[_ROLL];
  Vcan_Buff.DataBuf[5]=NamelessQuad.Speed[_ROLL];
  Vcan_Buff.DataBuf[6]=GPS_Vel.N;
  Vcan_Buff.DataBuf[7]=Earth_Frame_To_XYZ.N;
*/
  //Vcan_Buff.DataBuf[0]=Acce_Correct[0];
  //Vcan_Buff.DataBuf[1]=Acce_Correct[1];
  //Vcan_Buff.DataBuf[2]=Acce_Correct[2];
  //Vcan_Buff.DataBuf[3]=imu.accelRaw[0];
  //Vcan_Buff.DataBuf[4]=imu.accelRaw[1];
 // Vcan_Buff.DataBuf[6]=imu.accelRaw[2];
/*
  Vcan_Buff.DataBuf[0]=ADRC_Roll_Controller.x1;
  Vcan_Buff.DataBuf[1]=ADRC_Roll_Controller.x2;
  Vcan_Buff.DataBuf[2]=Total_Controler.Roll_Angle_Control.Control_OutPut;

  Vcan_Buff.DataBuf[3]=Roll_Gyro;
  Vcan_Buff.DataBuf[4]=ADRC_Roll_Controller.z1;
  Vcan_Buff.DataBuf[5]=ADRC_Roll_Controller.z2;
  Vcan_Buff.DataBuf[6]=Total_Controler.Roll_Gyro_Control.Control_OutPut;
  Vcan_Buff.DataBuf[7]=ADRC_Roll_Controller.u0;
  //Vcan_Buff.DataBuf[7]=FilterBefore_NamelessQuad.Acceleration[_YAW];
  //Vcan_Buff.DataBuf[7]=Gyro_Delta_Length;
  //Vcan_Buff.DataBuf[6]=Acceleration_Length;
*/
  Vcan_Buff.End=0x000003fc;
  Quad_DMA1_USART1_SEND((u32)(&Vcan_Buff),sizeof(Vcan_Buff));
  DMA_Send_Cnt=0;
  }
}
首先定义一个DMA_Vcan_Buff型的结构体Vcan_Buff：

DMA_Vcan_Buff的定义：

typedef struct
{
  uint32_t Head;
  float DataBuf[8];
  uint32_t End;
}DMA_Vcan_Buff;
函数具体语句： 

  static uint16_t DMA_Send_Cnt=0;
  DMA_Send_Cnt++;
定义一个静态变量，自加；判断 

 if(DMA_Send_Cnt>=DMA_SEND_PERIOD)
主要语句就几句其他都是注释 

Vcan_Buff.Head=0xfc030000;
Vcan_Buff.DataBuf[0]=NamelessQuad.Position[_YAW];
  Vcan_Buff.DataBuf[1]=NamelessQuad.Speed[_YAW];
  Vcan_Buff.DataBuf[2]=NamelessQuad.Acceleration[_YAW];
  Vcan_Buff.DataBuf[3]=Altitude_Estimate;
  Vcan_Buff.DataBuf[4]=Origion_NamelessQuad.Acceleration[_YAW];
  Vcan_Buff.DataBuf[5]=acc_correction[_YAW];
  Vcan_Buff.DataBuf[7]=vel_correction[_YAW];
  Vcan_Buff.DataBuf[6]=pos_correction[_YAW];
Vcan_Buff.End=0x000003fc;
这几句是对 Vcan_Buff的定义，可以说是待发送的数据 

 Quad_DMA1_USART1_SEND((u32)(&Vcan_Buff),sizeof(Vcan_Buff));
 DMA_Send_Cnt=0;
Quad_DMA1_USART1_SEND发送数据

DMA_Send_Cnt=0;又赋零，这个数没什么存在感

Quad_DMA1_USART1_SEND的定义：

void    Quad_DMA1_USART1_SEND(u32 SendBuff,u16 len)//DMA---USART1传输
{
	Quad_DMA_Config(DMA1_Channel4,(u32)&USART1->DR,(u32)SendBuff,len);//DMA1通道4,外设为串口1,存储器为SendBuff,长度len.
	USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);
	Quad_DMA_Enable(DMA1_Channel4);
	//while(DMA_GetFlagStatus(DMA1_FLAG_TC4) != SET);
	//DMA_ClearFlag(DMA1_FLAG_TC4);//清除发送完成标志
}
先看看Quad_DMA_Config，初始化DMA，DMA-USART1 

void Quad_DMA_Config(DMA_Channel_TypeDef* DMA_CHx,u32 cpar,u32 cmar,u16 cndtr)
{
 	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);	//使能DMA传输，挂载DMA1时钟

        DMA_DeInit(DMA_CHx);   //将DMA的通道1寄存器重设为缺省值，这里是DMA1_Channel4
	DMA1_MEM_LEN=cndtr;
	DMA_InitStructure.DMA_PeripheralBaseAddr = cpar;  //DMA外设ADC基地址
	DMA_InitStructure.DMA_MemoryBaseAddr =cmar;//DMA内存基地址
	DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;  //外设作为数据传输的目的地
	DMA_InitStructure.DMA_BufferSize = cndtr;  //DMA通道的DMA缓存的大小
	DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;  //外设地址寄存器不变
	DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;  //内存地址寄存器递增
	DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;  //数据宽度为8位
	DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; //数据宽度为8位
	DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;  //工作在正常缓存模式
	DMA_InitStructure.DMA_Priority = DMA_Priority_Medium; //DMA通道 x拥有中优先级
	DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;  //DMA通道x没有设置为内存到内存传输
	DMA_Init(DMA_CHx, &DMA_InitStructure);  //根据DMA_InitStruct中指定的参数初始化DMA的通道USART1_Tx_DMA_Channel所标识的寄存器

}

其中u16 DMA1_MEM_LEN;//保存DMA每次数据传送的长度

接着 
	USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);//使能指定USART的DMA请求
	Quad_DMA_Enable(DMA1_Channel4);
第一句好理解

第二句

void Quad_DMA_Enable(DMA_Channel_TypeDef*DMA_CHx)//开启一次DMA传输
{
	DMA_Cmd(DMA_CHx, DISABLE );
        //关闭USART1 TX DMA1 所指示的通道
        DMA_InitStructure.DMA_BufferSize =DMA1_MEM_LEN;
	DMA_Init(DMA1_Channel4, &DMA_InitStructure);
 	DMA_Cmd(DMA_CHx, ENABLE);  //使能USART1 TX DMA1 所指示的通道
}

先关闭DMA1 所指示的通道，在复制后DMA_InitStructure.DMA_BufferSize =DMA1_MEM_LEN;初始化DMA ,又打开DMA1 所指示的通道

下面应该回到ANO_SEND_StateMachine

这个函数复杂一点就有：

int16_t ANO_Cnt=0;
void ANO_SEND_StateMachine()
{
 ANO_Cnt++;
 if(ANO_Cnt==1)
 {
    ANO_Data_Send_Status();
 }
 else if(ANO_Cnt==2)
 {
    ANO_DT_Send_Senser((int16_t)X_g_av,(int16_t)Y_g_av,(int16_t)Z_g_av,
                       (int16_t)X_w_av,(int16_t)Y_w_av,(int16_t)Z_w_av,
                       (int16_t)DataMag.x,(int16_t)DataMag.y,(int16_t)DataMag.z);
 }
 else if(ANO_Cnt==3)
 {
  ANO_DT_Send_RCData(PPM_Databuf[2],PPM_Databuf[3],
                     PPM_Databuf[0],PPM_Databuf[1],
                     PPM_Databuf[4],PPM_Databuf[5],
                     PPM_Databuf[6],PPM_Databuf[7],0,0);
 }
 else if(ANO_Cnt==4)
 {
    ANO_DT_Send_GPSData(1,GPS_Sate_Num,Longitude_Origion,Latitude_Origion,10);
    ANO_Cnt=0;
 }
  else if(ANO_Cnt==5)
 {
    ANO_Cnt=0;
 }
}
这个发送的数据比较多，根据ANO_Cnt的值来发送那个数据

首先当ANO_Cnt等于1时

运行

ANO_Data_Send_Status();
uint8_t data_to_send[50];
/************************************************************/
//1：发送基本信息（姿态、锁定状态）
void ANO_Data_Send_Status(void)
{
	u8 _cnt=0;
	vs16 _temp;
	vs32 _temp2;
	u8 sum = 0;
	u8 i;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x01;
	data_to_send[_cnt++]=0;

	_temp = (int)(Roll*100);
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = (int)(Pitch*100);
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = (int)(Yaw*100);
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);

	_temp2 = (vs32)(100*NamelessQuad.Position[_YAW]);//单位cm
	data_to_send[_cnt++]=BYTE3(_temp2);
	data_to_send[_cnt++]=BYTE2(_temp2);
	data_to_send[_cnt++]=BYTE1(_temp2);
	data_to_send[_cnt++]=BYTE0(_temp2);

        data_to_send[_cnt++]=0x01;//飞行模式
        data_to_send[_cnt++]=Controler_State;//上锁0、解锁1

	data_to_send[3] = _cnt-4;
	sum = 0;
	for(i=0;i<_cnt;i++)
		sum += data_to_send[i];
	data_to_send[_cnt++]=sum;
        Quad_DMA1_USART3_SEND((u32)(data_to_send),_cnt);
        //UART3_Send(data_to_send, _cnt);
}
这里面有几个不一样的定义 

//数据拆分宏定义，在发送大于1字节的数据类型时，比如int16、float等，需要把数据拆分成单独字节进行发送
#define BYTE0(dwTemp)       ( *( (char *)(&dwTemp)    ) )
#define BYTE1(dwTemp)       ( *( (char *)(&dwTemp) + 1) )
#define BYTE2(dwTemp)       ( *( (char *)(&dwTemp) + 2) )
#define BYTE3(dwTemp)       ( *( (char *)(&dwTemp) + 3) )

不是很理解，先放着吧！

看看data_to_send都是有啥

前四个，0xAA;0xAA;0x01;_cnt-4;（这个应该是11）横滚俯仰偏航高度模式上锁状态这些数据的和

后面一句

Quad_DMA1_USART3_SEND((u32)(data_to_send),_cnt);

定义

void    Quad_DMA1_USART3_SEND(u32 SendBuff,u16 len)//DMA---USART1传输
{
	Quad_DMA1_Config(DMA1_Channel2,(u32)&USART3->DR,(u32)SendBuff,len);//DMA1通道4,外设为串口1,存储器为SendBuff,长度len.
	USART_DMACmd(USART3, USART_DMAReq_Tx, ENABLE);
	Quad_DMA_Enable(DMA1_Channel2);
	//while(DMA_GetFlagStatus(DMA1_FLAG_TC4) != SET);
	//DMA_ClearFlag(DMA1_FLAG_TC4);//清除发送完成标志
}
和Quad_DMA1_USART1_SEND类似，这个只不过初始化的时候初始化了DMA1_Channel2通道和USART3 

void Quad_DMA1_Config(DMA_Channel_TypeDef* DMA_CHx,u32 cpar,u32 cmar,u16 cndtr)
{
        RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);	//使能DMA传输
        DMA_DeInit(DMA_CHx);   //将DMA的通道1寄存器重设为缺省值
	DMA1_MEM_LEN=cndtr;
	DMA_InitStructure.DMA_PeripheralBaseAddr = cpar;  //DMA外设ADC基地址
	DMA_InitStructure.DMA_MemoryBaseAddr =cmar;//DMA内存基地址
	DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;  //外设作为数据传输的目的地
	DMA_InitStructure.DMA_BufferSize = cndtr;  //DMA通道的DMA缓存的大小
	DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;  //外设地址寄存器不变
	DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;  //内存地址寄存器递增
	DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;  //数据宽度为8位
	DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; //数据宽度为8位
	DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;  //工作在正常缓存模式
	DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh; //DMA通道 x拥有中优先级
	DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;  //DMA通道x没有设置为内存到内存传输
	DMA_Init(DMA_CHx, &DMA_InitStructure);  //根据DMA_InitStruct中指定的参数初始化DMA的通道USART1_Tx_DMA_Channel所标识的寄存器
}
可以类比


ANO_Cnt==2时

ANO_DT_Send_Senser((int16_t)X_g_av,(int16_t)Y_g_av,(int16_t)Z_g_av,
                       (int16_t)X_w_av,(int16_t)Y_w_av,(int16_t)Z_w_av,
                       (int16_t)DataMag.x,(int16_t)DataMag.y,(int16_t)DataMag.z);
打开定义 

void ANO_DT_Send_Senser(s16 a_x,s16 a_y,s16 a_z,s16 g_x,s16 g_y,s16 g_z,s16 m_x,s16 m_y,s16 m_z)
{
    u8 _cnt=0;
    vs16 _temp;
    u8 sum = 0;
    u8 i=0;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0x02;
    data_to_send[_cnt++]=0;

    _temp = a_x;
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);
    _temp = a_y;
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);
    _temp = a_z;
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);

    _temp = g_x;
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);
    _temp = g_y;
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);
    _temp = g_z;
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);

    _temp = m_x;
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);
    _temp = m_y;
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);
    _temp = m_z;
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);

    data_to_send[3] = _cnt-4;

    sum = 0;
    for(i=0;i<_cnt;i++)
        sum += data_to_send[i];
    data_to_send[_cnt++] = sum;
    Quad_DMA1_USART3_SEND((u32)(data_to_send),_cnt);
    //UART3_Send(data_to_send, _cnt);
}
这个发送和ANO_Data_Send_Status类似发送三个轴的加速度，角速度，磁力计数据

ANO_Cnt==3时发送接收机接收到的数据

ANO_DT_Send_RCData(PPM_Databuf[2],PPM_Databuf[3],
                     PPM_Databuf[0],PPM_Databuf[1],
                     PPM_Databuf[4],PPM_Databuf[5],
                     PPM_Databuf[6],PPM_Databuf[7],0,0);
 }
void ANO_DT_Send_RCData(u16 thr,u16 yaw,u16 rol,u16 pit,u16 aux1,u16 aux2,u16 aux3,u16 aux4,u16 aux5,u16 aux6)
{
    u8 _cnt=0;
    u8 i=0;
    u8 sum = 0;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0x03;
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=BYTE1(thr);
    data_to_send[_cnt++]=BYTE0(thr);
    data_to_send[_cnt++]=BYTE1(yaw);
    data_to_send[_cnt++]=BYTE0(yaw);
    data_to_send[_cnt++]=BYTE1(rol);
    data_to_send[_cnt++]=BYTE0(rol);
    data_to_send[_cnt++]=BYTE1(pit);
    data_to_send[_cnt++]=BYTE0(pit);
    data_to_send[_cnt++]=BYTE1(aux1);
    data_to_send[_cnt++]=BYTE0(aux1);
    data_to_send[_cnt++]=BYTE1(aux2);
    data_to_send[_cnt++]=BYTE0(aux2);
    data_to_send[_cnt++]=BYTE1(aux3);
    data_to_send[_cnt++]=BYTE0(aux3);
    data_to_send[_cnt++]=BYTE1(aux4);
    data_to_send[_cnt++]=BYTE0(aux4);
    data_to_send[_cnt++]=BYTE1(aux5);
    data_to_send[_cnt++]=BYTE0(aux5);
    data_to_send[_cnt++]=BYTE1(aux6);
    data_to_send[_cnt++]=BYTE0(aux6);

    data_to_send[3] = _cnt-4;

    sum = 0;
    for(i=0;i<_cnt;i++)
        sum += data_to_send[i];

    data_to_send[_cnt++]=sum;
    Quad_DMA1_USART3_SEND((u32)(data_to_send),_cnt);
    //UART3_Send(data_to_send, _cnt);
}
ANO_Cnt==4时发送GPS的数据，后 ANO_Cnt=0 

ANO_DT_Send_GPSData(1,GPS_Sate_Num,Longitude_Origion,Latitude_Origion,10);

void ANO_DT_Send_GPSData(u8 Fixstate,
                          u8 GPS_Num,
                          u32 log,
                          u32 lat,
                          int16 gps_head)
{
    u8 sum = 0;
    u8 _cnt=0;
    u8 i=0;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0x04;
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=Fixstate;
    data_to_send[_cnt++]=GPS_Num;

    data_to_send[_cnt++]=BYTE3(log);
    data_to_send[_cnt++]=BYTE2(log);
    data_to_send[_cnt++]=BYTE1(log);
    data_to_send[_cnt++]=BYTE0(log);

    data_to_send[_cnt++]=BYTE3(lat);
    data_to_send[_cnt++]=BYTE2(lat);
    data_to_send[_cnt++]=BYTE1(lat);
    data_to_send[_cnt++]=BYTE0(lat);

    data_to_send[_cnt++]=BYTE1(gps_head);
    data_to_send[_cnt++]=BYTE0(gps_head);

    data_to_send[3] = _cnt-4;

    sum = 0;
    for(i=0;i<_cnt;i++)
        sum += data_to_send[i];

    data_to_send[_cnt++]=sum;
    Quad_DMA1_USART3_SEND((u32)(data_to_send),_cnt);
    //UART3_Send(data_to_send, _cnt);
}
就先写到这
















JRlipton
关注
匿名飞控代码源码-2015
03-01
此代码为匿名飞控代码2015版 适合学习飞控
无名飞控c语言源码,stm32飞控
weixin_39972019的博客
05-28 701
很不错的飞控float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientationfloat exInt = 0, eyInt = 0, ezInt = 0; // scaled integral errorvoid IMUupdate(T_int16_xyz...
无名的飞控默认是用的PID，没用ADRC，可以改变宏定义切换。而且只在角速度环用ADRC。
诗筱涵的博客
10-01 1366
看了下代码，默认是PID模式，所以实际那飞的效果是PID。 而且从它这宏定义看书，无名只是把ADRC用在角速度环上面，角速度环的控制可以用ADRC或者PID。 我是怎么一步步找到那的呢，先找到main.c 这里说 主要核心：传感器滤波、姿态解算、惯导、控制等代码在TIME.c里面运行 那就找到time.c 可以看到最关键的一段，把整个控制算法都概括了！！！！ Total_Control();//总控制器：水平位置+水平速度+姿态（角度+角速度）控制器，高度...
无名创新飞控开源地面站软件NGroundStation源码发布说明
热门推荐
NamelessCotrunQaud的博客
04-06 1万+
无名创新飞控开源地面站——NGroundStation 一、地面站开源初衷与下载链接： 无名创新飞控开源飞控地面站是无名小哥为广大自研飞控开发者、电子发烧友们专门开发的一款多功能上位机软件，地面站功能涵盖：飞控状态显示、数据波形、参数调试、虚拟遥控器、飞控传感器校准串口助手常用等，开发此款地面站的初衷是方便大家高效学习与调试。 初学者在学习无人机飞控相关算法选...
什么是DMA？（转）
chance_wang的专栏
04-18 2054
FROM: Soul of Angel什么是DMA？ 当我们向计算机中加入了一块新的声卡或其它适配卡时，安装程序可能会提醒我们应该选择一个DMA通道。那DMA是什么呢? DMA（Direct Memory Access），即直接存储器存取，是一种快速传送数据的机制。数据传递可以从适配卡到内存，从内存到适配卡或从一段内存到另一段内存。DMA技术的重要 性在于，利用它进行数据传送时不需要CPU的参与
labview+mpu6050 飞控姿态解算与3D显示.rar
12-19
MPU6050是一款集成了加速度计和陀螺仪的六轴传感器，能够检测物体在三维空间中的线性加速度和角速度，是无人机、机器人等移动平台姿态控制的关键组件。其内部包含一个三轴加速度计和一个三轴陀螺仪，可以测量物体的...
STM32 DIY 飞控 姿态解算原理.rar
02-20
免责声明：资料部分来源于合法的互联网渠道收集和整理，部分自己学习积累成果，供大家学习参考与交流。收取的费用仅用于收集和整理资料耗费时间的酬劳。 本人尊重原创作者或出版方，资料版权归原作者或出版方所有，...
TI杯参赛者福利—四轴飞控板姿态解算（原理图、源代码开源）-电路方案
04-22
四轴飞控板电路图 PCB图 原理图说明: 1、主控MCU采用MSP430F5438A； 2、采用25MHZ外部晶振； 3、加速度计和陀螺仪采用MPU6050； 4、磁力计采用HMC5883L； 5、添加了四个LED灯，两个按键和一个蜂鸣器； 6、集成了CP...
Fly.zip_MPU6050 姿态_mpu6050 飞控_和6050通讯方式_姿态解算_飞控
07-14
飞控源码，主要讲解Mpu6050的通信方式和姿态解算。。。
px4飞控EKF姿态解算方法
12-01
PX4飞控系统是一款开源的无人机飞控软件，广泛应用于各种无人机和飞行器项目中。PX4利用扩展卡尔曼滤波器（EKF）...通过上述对EKF算法的理解和应用，PX4飞控得以实现准确的飞行姿态控制，从而完成各种复杂的飞行任务。
无名科创开源飞控-电路方案
04-21
开源飞控:支持气压计、超声波定高、户外GPS定点、定速巡航，部分视频链接如下: 无名飞控暴力测试 无名飞控江边定点（长时间） 无名飞控定高作死测试 无名飞控加速度计6面校准与融合简单讲解 无名飞控源码整体框架介绍初步 无名飞控解（上）锁与遥控器简单设置 无名科创自研飞控平台，经过武汉科技大学连续四届研究生师兄们参考国内外主流飞控（APM、Pixhawk、Autoquad、MWC、CC3D、无穷开、ANO、Light等）的算法与整体框架的进行深入学习基础上，经过软、硬件的精心设计，继承与发展，目前飞控整体功能相对完善，主要功能有:姿态自稳、超声波、气压计定高，户外GPS定点，GPS模式下定速巡航等功能，涵盖飞控学习主要核心算法: 1、四旋翼的传感器滤波(针对传感器不同使用情况:姿态解算、惯导、控制、传感器矫正等)分别采用窗口滑动滤波、不同截止频率的巴特沃斯数字低通滤波器)；2、姿态解算(互补滤波、梯度下降法等)；3、惯性导航（经典回路反馈法即APM三阶段互补滤波，单观测器的卡尔曼滤波，双观测量的卡尔曼滤波，观测传感器延时修正处理等）； 4、控制算法(经典PID控制、前馈控制、自抗扰控制ADRC)等。 无名科创团队的发展: 多旋翼飞行器飞行控制系统（简称飞控）是我们团队历届主研项目，团队13年即开始第一代飞控的研究，从最开始的小四轴，到后来的多旋翼飞控，经历N个版本改进，经历无数次断桨、射桨、炸机，一步一步完善与改进，整合除了目前我们这款对外开源的飞控。团队历来贡献者均就职于无人机公司做算法相关工作。目前我们的飞控更加完善，更加稳定，更加适合学习，主要核心代码自写率达到百分之90以上，代码基本上是逐行注释，整个飞控框架清晰明了，模块化封装规范，方便大家学习与二次开发。由于作者目前仍然在校，主研项目仍为飞控，个人时间比较多，可和大家一同交流学习。我们的服务宗旨是:打造国内功能最多、性能最好、成本最低、可玩性最强的开源飞控学习平台。帮助大家以最小的代价、最大的获得感、最快的速度学习飞控相关算法，顺利完成进阶逆袭！！！ 团队主要成员CSDN开源技术博客汇总 ，充分展现我们开源共享、共同进步的创客精神，不废话了，直接上图: 部分技术博客截图: 无名科创开源飞控:独家首创10轴IMU组合:MPU6050（加速度计、陀螺仪）+IST8310（DJI同款磁力计）+SPL06-001（歌尔高精度气压计、媲美MS5611），MCU:STM32F103RCT6，这是一款强大的飞控，主频72Mhz，flash 256K，板载10 Axis传感器，3轴陀螺，3轴加速度，3轴磁罗盘，高精度气压计。适合新手学习无刷飞控，更适合玩家做多种拓展和二次开发，飞控预留多个串口，可外接各种附加设备，已实现超声波定高、气压计定高、GPS定点等功能，代码完全开源。 无名科创开源飞控学习平台: 1、飞控板与IMU分离式设计 2、采用3D打印的IMU气压防护罩 3、软件调试支持主流IAR、Keil两款编译器 4、支持多家上位机与地面站，方便调试
无名创新飞控开源地面站NGroundStation
03-22
无名创新飞控开源地面站NGroundStation，支持状态显示、参数调试、波形显示、数据导入导出、虚拟遥控器、一键起飞、降落、SDK控制、采用QT开发，软件持续更新
电子-无名科创开源飞控使用说明书V2.0.6.pdf
09-05
电子-无名科创开源飞控使用说明书V2.0.6.pdf,综合电子技术四轴飞行器|飞控
【官方开源】飞控主板+摇控手柄+2.4G RF module等全套设计资料-电路方案
04-21
可能感兴趣的项目设计:https://www.cirmall.com/circuit/3399/detail?3（新唐M452设计的四轴飞控板电路原理图、PCB、源代码、解决方案和APP） 新唐飞控系统组成: 概述: 此套件包含了飞控主板、摇控手柄、2.4G RF module、分电板外加 Nu-Link-Me 仿真器，使用新唐 ARM:registered: Cortex:registered:-M4 开发板；本套件拥有完整姿态运算数据库 (AHRS Library) ，让玩家简易上手，不须钻研传感器姿态运算的数学问题，更容易专注于飞行控制以及飞行应用的开发。搭配机架、电变、马达、螺旋桨及背载锂电池即可自由飞行。 本套件支持失控保护(自动降落)，支持定高(气压计)及无头模式(电子罗盘)，可使用 2.4G 专用遥控模块或是市售通用发射机接收机，且提供使用者依照各式大小轴距机架进而自行调适飞行姿态PID(Proportional Integrative Derivative) 及校准传感器。 飞控硬件框架: 新塘飞控系统方案特点: 一站解决:本套件含 Cortex:registered:-M4 为核心的主控板和 2.4G 无线传输遥控板，装机、上电后无需设定。 独家数据库:提供完整姿态运算数据库 (AHRS Library) ，玩家可以快速自行设定。 弹性尺寸:默认支持的套件配置 (250mm) 外，玩家也可自行挑选大轴距 (450mm) 机架与电机，自行调整飞控板PID，即可驾驭不同配置的飞行器。 搭配 MPU6500 实现 6 轴飞行姿势控制。 Nuvoton AHRS 运算库架构: 全套设计资料包括整个硬件设计，程序源码&开发工具，同时包括文档详细说明。 整个硬件设计截图，用PADS9.5打开: 程序源码下载:
无名创新TIVA LaunchPad V2版本飞控代码阶段性更新日志
NamelessCotrunQaud的博客
05-08 1756
TIVA LaunchPad V2版本飞控代码阶段性更新日志
四轴飞控源码
05-19
PID算法程序 基于四轴飞行器 CPU: STM32F103CB2.4G: NRF24L01电子罗盘：HMC5883陀螺仪+加速度计：MPU-6050 固定的传感器通讯格式为:0X88+0XA1+0X1D+ACC XYZ+GYRO XYZ +MAG XYZ +ANGLE ROLL PITCH YAW + cyc_time +0x00 + 0x00 +0x00 注意,所有数据位int16格式,angle飞控端为float,发送时乘以100,上位机以int16格式接收,显示时再除以100 自定义通讯格式为:0x88+自定义功能字如0xf1+lengh+data
黑洞飞控代码分析（三）------姿态解算和控制
lipton的博客
01-08 1650
在收集需要的数据后，进行处理，解算姿态 在进入之前先看一看系统时钟的中断函数 /* * 函数名：SysTick_Handler * 描述 ：滴答中断函数 * 输入 ：无 * 输出 ：无 */ void SysTick_Handler(void) { static u16 cnt = 0; static u8 ms = 0; cnt++; Prepare_
无名的ADRC的C语言实现
诗筱涵的博客
09-26 9395
分为ADRC.h和ADRC.c #ifndef _ADRC_H_ #define _ADRC_H_ /*----------------------------------------------------------------------------------------------------------------------/ * 本程序只供购买者学习使用，版权著作权属于无名科创团队， * 无名科...
labview+mpu6050飞控姿态解算与3
最新发布
11-24
同时使用LabVIEW编写上位机，可以实现飞控姿态解算与3D可视化。下面是一个简单的流程： 1.使用Arduino连接MPU6050，读取传感器数据。 2.将传感器数据通过串口发送给LabVIEW上位机。 3.LabVIEW上位机接收到数据后，...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值