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

2023-05-16

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

 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);
}

就先写到这

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

无名飞控的姿态解算和控制

无名飞控的姿态解算和控制（四）的相关文章

【谷歌面试题】求数组中两个元素的最小距离

一个数组 xff0c 含有重复元素 xff0c 给出两个数num1和num2 xff0c 求这两个数字在数组中出现的位置的最小距离 O n 时间复杂度 xff0c O 1 空间复杂度 int minDistance int A int si
进程间通信

原作者地址不详摘要随着人们对应用程序的要求越来越高 xff0c 单进程应用在许多场合已不能满足人们的要求编写多进程多线程程序成为现代程序设计的一个重要特点 xff0c 在多进程程序设计中 xff0c 进程间的通信是不可避免的 Mi
通过能观性分析理解SLAM系统的可观维度。

目录 1 能观性分析大体理解2 可观性定义3 可观性矩阵 1 能观性分析大体理解什么是能观性分析 xff1f 能观性分析通过计算可观性矩阵 xff0c 分析它的零空间的秩 xff0c 来分析系统哪些状态维度可观不可观可观性矩阵对应系统
百度2014移动研发笔试题目——1013清华版

一简答题 1 简述计算机的存储系统分为哪几个层次 xff0c 为什么这样的分层能够提高程序的执行效率 2 浮点数在计算中如何表示 xff0c 如何对浮点数判等 3 简述TCP与UDP协议的差别 xff0c 两者与HTTP的关系并列举HT
史上最全的ubuntu16.04安装nvidia驱动+cuda9.0+cuDnn7.0

本文参考了 http www cnblogs com 5211314jackrose p 7081020 html https jingyan baidu com article 4853e1e55679491909f726f4 html
4、ORB-SLAM闭环检测之通过词袋寻找当前关键帧和闭环候选帧之间的匹配

目录 1 SearchByBoW 2 图解每一步实现原理 2 1 通过词袋模型寻找匹配 2 2 通过旋转直方图来筛除离群点 3 期待您加入前面我们已经了解到了sim3的求解流程具体计算过程中有三步比较重要 1 寻找两关键帧之间的粗匹配
基于Web服务的物联网-WoT(Web of Things)

当我们谈到智能手机 xff0c 多样性往往不是问题 xff0c 主流市场不是基于苹果IOS系统的就是谷歌的Android系统 xff0c 但即将到来的物联网浪潮 xff0c 智能对象是否也能这样 xff1f 这就复杂多了 xff0c 当前
如何选择 catkin_make和catkin_make_isolated

问题在编译cartographer的代码包的时候 xff0c 会被建议使用catkin make isolated xff0c 为何不使用catkin make呢 xff1f 原因 catkin make was the first sc
基于共享内存的分布式消息学习笔记

作者 xff1a 深圳自研业务组 jimwu 编辑 xff1a 上海业务组 alenai 目录 xff1a Tbus 简介 Tbus 原理 Tbus 配置与工具 Tbus 简单应用 Tbus 运维应用为 python 扩展总结 Tbus
分享一下工作以来我看过计算机书籍

由于自工作依赖一直专注于linux 下的c c 43 43 编程工作 xff0c 所以 xff0c 我的书籍也大的都是这方这边书尽管很经典 xff0c 但是我的能力实在有限 xff0c 只把数据结构的那点看了一下 xff0c 其他的看的
51单片机定时器初值计算详解

前言 xff1a 本文详细介绍了51单片机学习过程中定时器的初值计算问题以及相关概念 xff0c 力求把每一个学习过程中的可能会遇到的难点说清楚 xff0c 并举相关的例子加以说明学习完毕 xff0c 又顺手利用刚学到定时器的相关知识写了
STM32平台下官方DMP库6.12超详细移植教程

前记 Motion Driver官方库 xff1a Motion Driver 6 12 STM32工程源码 xff1a STM32F103C8 软件MPU6050 xff08 DMP xff09 MPU6050软件I2C驱动 xff0c
STM32F103C8-平衡小车笔记

STM32F103C8 平衡小车笔记 1 PID的作用 xff08 1 xff09 比例项 xff1a 提高响应速度 xff0c 减小静差 xff08 2 xff09 积分项 xff1a 消除稳态误差 xff08 3 xff09 微分项 x
嵌入式Linux系统开发笔记（十四）

U Boot环境变量 uboot 中有两个非常重要的环境变量 bootcmd 和 bootargs xff0c bootcmd 和 bootagrs 是采用类似 shell 脚本语言编写的 xff0c 里面有很多的变量引用 xff0c 这些
嵌入式Linux系统开发笔记（十五）

Linux内核启动验证 5 1 编译内核 span class token comment 清除工程 span span class token comment make distclean span span class token co
基于ROS搭建机器人仿真环境

别人的发复现及经验 https blog csdn net qq 38620941 article details 125321347 gazebo默认仿真环境 1 gazebo models 是系统下gazebo放置模型库的默认位置 2
嵌入式Linux系统开发笔记（十六）

根文件系统rootfs启动验证测试接下来我们使用测试一下前面创建好的根文件系统 rootfs xff0c 测试方法使用 NFS 挂载 6 1 检查是否在Ubuntu主机中安装和开启了NFS服务 xff08 特别注意 xff1a nfs 配
安卓5.0以上7.0以下使用Termux

参考 xff1a https zhuanlan zhihu com p 400507701 说明 xff1a Termux支持5 0以上的安卓系统 Termux7 3版本之后 xff0c 仅支持7 0以上的安卓系统 1 安装Termux 设
关于DSP的CCS6.0平台下的工程搭建（完全可移植）

本工程以CCS6 0下新建TMS320F28335工程为例 xff0c 其他系列处理器工程搭建类似 xff0c 参考本例即可工程搭建用到的F2833x TI官方库文件下载链接也可直接参考笔者搭建好CCS6 0的工程下载链接所建工程

随机推荐

STM32Fxx JTAG/SWD复用功能重映射

问题描述 xff1a 在实验室调车过程中 xff0c 遇到的一个问题 xff1a 为了每次下载程序方便 xff0c 队员们往往会把 Jlink 插在板子上 xff0c 可是在调车过程中发现 xff0c 有时程序会莫名死掉 xff0c 而同样
VS2012编译RTKLIB——GNSS定位开源库

RTKLIB 开源库有着强大的 GPS 数据实时和后处理功能 xff0c 由于笔者的毕业设计中需要对GPS 载波相位观测量进行 RTK 解算 xff0c 故而 xff0c 对 RTKLIB 开源库进行了学习与研究 RTKLIB 提供了很
51单片机串行口波特率计算

1 工作方式介绍 xff1a 方式 0 xff1a 这种工作方式比较特殊 xff0c 与常见的微型计算机的串行口不同 xff0c 它又叫同步移位寄存器输出方式在这种方式下 xff0c 数据从 RXD 端串行输出或输入 xff0c 同步信
数码管显示问题总结

1 数码管显示原理我们最常用的是七段式和八段式 LED 数码管 xff0c 八段比七段多了一个小数点 xff0c 其他的基本相同所谓的八段就是指数码管里有八个小 LED 发光二极管 xff0c 通过控制不同的 LED 的亮灭来显示出不同
单片机与嵌入式linux 比较

MCU门槛低 xff0c 入门容易 xff0c 但是灵活 xff0c 其实对工程师的软硬件功底要求更高 xff0c 随着半导体的飞速发展 xff0c MCU能实现很多匪夷所思匪夷所思的功能 xff0c 比如 xff0c 使用GPIO模拟1个
rtk 精确定位简介

RTK又称载波相位差分 xff1a 基准站通过数据链及时将其载波观测量及站坐标信息一同传送给用户站用户站接收GPS卫星的载波相位与来自基准站的载波相位 xff0c 并组成相位差分观测值进行及时处理 xff0c 能及时给出厘米级的定位结果
STM32开发利器：STM32CubeMX

这篇博客篇幅不长 xff0c 主要是为大家介绍ST公司推出的STM32CubeMX开发工具 xff0c 当成下周更新STM32 10个项目工程的预备篇 xff0c 同时FPGA FPGA 20个例程篇 xff1a 8 SD卡任意地址的读写
ROS命名空间

ROS命令空间是一个很重要的内容 xff0c 官方文档 xff1a http wiki ros org Names 分为三类 xff1a relative xff0c global xff0c private 下边是一个官网给的示例 Nod
STM32CubeMX关于添加DSP库的使用

前言人生如逆旅 xff0c 我亦是行人一介绍 STM32 系列基于专为要求高性能低成本低功耗的嵌入式应用专门设计的 ARM Cortex M3 内核而 DSP 应该是 TMS320 系列 xff0c TMS320 系列 DSP
STM32H750VBT6的DSP使用的学习——基于CubeMX

前言人生如逆旅 xff0c 我亦是行人 1 STM32H7的DSP功能介绍 xff08 STMicroelectronics xff0c 简称ST xff09 推出新的运算性能创记录的H7系列微控制器新系列内置STM32平台中存储容量最
ROS中激光雷达数据类型传递转换及自定义点云数据类型介绍

目录一 ROS中激光雷达数据类型传递转换 xff1b 二点云数据解析三自定义点云数据类型一 ROS中激光雷达数据类型传递转换 xff1b ROS中涉及激光雷达传递的消息类型有两种 xff0c 一种是针对2D雷达 sensor ms
C/C++优秀书籍清单

转载自 xff1a https www cnblogs com kimiway p 3225767 html 书籍是码农进步的阶梯读好书好读书干一行爱一行除了工作还有生活在陪伴家人同时也不忘提高自己为更好的生活努力 1 C程序
打印_battery_status.scale

在px4的姿态控制中 xff0c publish控制量时代码乘以了一个 battery status scale xff0c scale effort by battery status if params bat scale en amp
无名飞控

无名飞控Time c文件由于无名飞控主要核心 xff1a 传感器滤波姿态解算惯导控制等代码在TIME c里面运行 xff0c 所以先来分析这个文件打开文件第一个函数 xff1a void Timer4 Configuration
无名飞控姿态解算和控制（一）

无名飞控的姿态解算和控制从imu和磁力计 xff0c 气压计拿到数据后 xff0c 进入AHRSUpdate GraDes Delayback函数 xff0c 其中X w av Y w av Z w av来自陀螺仪 xff0c X g a
无名飞控姿态解算和控制（三）

继续码代码上一篇主要写了自稳模式下的代码流程 xff0c 这次主要是飞控的定高和定点控制流程首先是定高控制模式在Main Leading Control里选择定高模式代码 xff1a else if Controler Mode 6
无名飞控框架梳理

打开飞控的main c文件首先是HardWave Init 飞控板内部资源相关外设初始化打开 include 34 Headfile h 34 Sensor Okay Flag Sensor Init Flag void HardWa
无名飞控的时钟和延时

首先是飞控里面调用了 SystemInit 时钟初始化这个里面 void SystemInit void Reset the RCC clock configuration to the default reset state for de
谈谈bit位序的问题

Linux内核里面有下面代码 struct iphdr if defined LITTLE ENDIAN BITFIELD u8 ihl 4 version 4 elif defined BIG ENDIAN BITFIELD u8 ver
无名飞控的姿态解算和控制（四）

上面几篇帖子已经写完控制流程还剩一点没说 if PPM Isr Cnt 61 61 100 PPM接收正常才进行传感器标定检测 Accel Calibration Check 加速度标定检测 Mag Calibration Check 磁力

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

无名飞控的姿态解算和控制（四） 的相关文章

随机推荐

热门标签

无名飞控的姿态解算和控制（四）的相关文章