java开发四轴飞行器上位机步骤_匿名四轴上位机和飞控经典STM32代码分享

/* Includes ------------------------------------------------------------------*/

#include "stm32f10x.h"

#include "sysconfig.h"

#include "bsp.h"

#include "led.h"

#include "tim3.h"

#include "usart.h"

#include "ANO_TC_STM32F1_I2C.h"

#include "MPU6050.h"

#include "moto.h"

#include "spi.h"

#include "nrf24l01.h"

#include "tim_pwm_in.h"

#include "rc.h"

#include "imu.h"

#include "control.h"

#include "data_transfer.h"

u8 SYS_INIT_OK=0;

void SYS_INIT(void)

{

LED_INIT();

LED_FLASH();

Moto_Init();

Uart1_Init(115200);

Tim3_Init(500);

#ifdef CONTROL_USE_RC

Tim_Pwm_In_Init();

#endif

Nvic_Init();

ANO_TC_I2C2_INIT(0xA6,400000,1,1,3,3);

MPU6050_Init();

Spi1_Init();

Nrf24l01_Init(MODEL_TX2,40);

if(Nrf24l01_Check())

Uart1_Put_String("NRF24L01 IS OK !\r\n");

else

Uart1_Put_String("NRF24L01 IS NOT OK !\r\n");

FLASH_Unlock();

EE_INIT();

EE_READ_ACC_OFFSET();

EE_READ_GYRO_OFFSET();

EE_READ_PID();

Tim3_Control(1);

}

u8 FLAG_ATT=0;

T_int16_xyz                 Acc,Gyr;        //两次综合后的传感器数据

T_int16_xyz                        Acc_AVG;

T_float_angle                 Att_Angle;        //ATT函数计算出的姿态角

vs32                                Alt;

T_RC_Data                         Rc_D;                //遥控通道数据

T_RC_Control                Rc_C;                //遥控功能数据

int main(void)

{

static u8 att_cnt=0;

static u8 rc_cnt=0;

static T_int16_xyz mpu6050_dataacc1,mpu6050_dataacc2,mpu6050_datagyr1,mpu6050_datagyr2;

static u8 senser_cnt=0,status_cnt=0,dt_rc_cnt=0,dt_moto_cnt=0;

SYS_INIT();

while (1)

{

if(FLAG_ATT)

{

FLAG_ATT = 0;

att_cnt++;

rc_cnt++;

if(rc_cnt==20)

{

rc_cnt = 0;

#ifdef CONTROL_USE_RC

Rc_GetValue(&Rc_D);

#endif

Rc_Fun(&Rc_D,&Rc_C);

}

if(att_cnt==1)

MPU6050_Dataanl(&mpu6050_dataacc1,&mpu6050_datagyr1);

else

{

att_cnt = 0;

MPU6050_Dataanl(&mpu6050_dataacc2,&mpu6050_datagyr2);

Acc.X = (mpu6050_dataacc1.X+mpu6050_dataacc2.X)/2;

Acc.Y = (mpu6050_dataacc1.Y+mpu6050_dataacc2.Y)/2;

Acc.Z = (mpu6050_dataacc1.Z+mpu6050_dataacc2.Z)/2;

Gyr.X = (mpu6050_datagyr1.X+mpu6050_datagyr2.X)/2;

Gyr.Y = (mpu6050_datagyr1.Y+mpu6050_datagyr2.Y)/2;

Gyr.Z = (mpu6050_datagyr1.Z+mpu6050_datagyr2.Z)/2;

Prepare_Data(&Acc,&Acc_AVG);

IMUupdate(&Gyr,&Acc_AVG,&Att_Angle);

Control(&Att_Angle,&Gyr,&Rc_D,Rc_C.ARMED);

if(Rc_C.ARMED)

LED1_ONOFF();

else

LED1_OFF;

senser_cnt++;

status_cnt++;

dt_rc_cnt++;

dt_moto_cnt++;

if(senser_cnt==5)

{

senser_cnt = 0;

Send_Senser = 1;

}

if(status_cnt==5)

{

status_cnt = 0;

Send_Status = 1;

}

if(dt_rc_cnt==10)

{

dt_rc_cnt=0;

Send_RCData = 1;

}

if(dt_moto_cnt==10)

{

dt_moto_cnt=0;

Send_MotoPwm = 1;

}

}

}

}

}