[资料分享与下载] 一起DIY四轴飞行器

int main(void)

    {

        uint32_t i;

        uint8_t ret;

        uint32_t counter = 0;

        imu_float_euler_angle_t angle;

        SYSTICK_DelayInit(); //Init Delay

        UART_QuickInit(UART1_RX_PC03_TX_PC04, 115200);  //Init UART and printf

        GPIO_QuickInit(HW_GPIOA, 1 , kGPIO_Mode_OPP);   //Init LED

        //Blink LED to indicate system is running

        for(i = 0; i < 10; i++)

        {

            GPIO_ToggleBit(HW_GPIOA, 1);

            DelayMs(50);

        }

        //init sensor

        ret = InitSensor();

        if(ret)

        {

            printf("Sensor init failed! code:%d\r\n", ret);

        }

        // install imu interface function

        imu_io_install(&IMU_IOInstallStruct1);

        // install trans uart for send data

        trans_io_install(&TRANS_IOInstallStruct1);

      

        trans_user_data_t send_data;

        while(1)

        {

            imu_get_euler_angle(&angle);

    //              通过串口将姿态的数据发送出去，串口速度为115200               

    //        printf("%05d %05d %05d\r", (int16_t)angle.imu_pitch, (int16_t)angle.imu_roll, (int16_t)angle.imu_yaw);

            

                            //以下函数用于发送数据帧，传输飞控需要的数据帧

            send_data.trans_pitch = (int16_t)angle.imu_pitch*100;

            send_data.trans_roll = (int16_t)angle.imu_roll*100;

            send_data.trans_yaw = (int16_t)angle.imu_yaw*10;

            trans_send_pactket(send_data);  //发送一帧数据

                            //结束

            //blink the LED

            counter++;

            counter %= 10;

            if(!counter)

            {

                GPIO_ToggleBit(HW_GPIOA, 1);   //LED运行灯闪烁

            }

        }

    }

    uint8_t InitSensor(void)

    {

        static int init = 0;

        uint8_t ret = 0;

        if(!init)

        {

            ret += mpu6050_init(&mpu6050_device1, IMU_TEST_I2C_MAP, "mpu6050", 96000);

            ret += hmc5883_init(&hmc_device, IMU_TEST_I2C_MAP, "hmc5883", 96000);

         //   ret += bmp180_init(&bmp180_device1, IMU_TEST_I2C_MAP, "bmp180", 96000);

            init = 1;

        }

        if(ret)

        {

            return ret;

        }

        return 0;

    }

    uint32_t imu_get_euler_angle(imu_float_euler_angle_t * angle)

    {

        uint8_t err = 0;

        int16_t ax,ay,az,gx,gy,gz,mx,my,mz;

        imu_raw_data_t raw_data;

    //    imu_raw_data_t filter_data;

        imu_float_data_t float_data;

        err += gpIOInstallStruct->imu_get_accel(&ax, &ay, &az);

        err += gpIOInstallStruct->imu_get_gyro(&gx, &gy, &gz);

        err += gpIOInstallStruct->imu_get_mag(&mx, &my, &mz);

        if(err >0)

        {

          return 1;

        }

        raw_data.ax = ax;

        raw_data.ay = ay;

        raw_data.az = az;

        raw_data.gx = gx;

        raw_data.gy = gy;

        raw_data.gz = gz;

        raw_data.mx = mx;

        raw_data.my = my;

        raw_data.mz = mz;

    #if 0

        //I need rawdata I give you filtered data

        imu_sliding_filter(raw_data, &filter_data);

    #endif

        // I need filtered data I give you float data

        imu_format_data(raw_data, &float_data);

        //I need float data I give you euler angles

        updateAHRS( float_data.gx * Gyro_Gr,

                    float_data.gy * Gyro_Gr,

                    float_data.gz * Gyro_Gr,

                    float_data.ax,

                    float_data.ay,

                    float_data.az,

                    float_data.mx,

                    float_data.my,

                    float_data.mz,

                    angle);

        return 0;

    }

    //飞行器姿态外环控制

    void ANO_FlyControl::Attitude_Outter_Loop(void)

    {

            int32_t        errorAngle[2];

            Vector3f Gyro_ADC;

            

            //计算角度误差值

            errorAngle[ROLL] = constrain_int32((rc.Command[ROLL] * 2) , -((int)FLYANGLE_MAX), +FLYANGLE_MAX) - imu.angle.x * 10;

            errorAngle[PITCH] = constrain_int32((rc.Command[PITCH] * 2) , -((int)FLYANGLE_MAX), +FLYANGLE_MAX) - imu.angle.y * 10;

            

            //获取角速度

            Gyro_ADC = mpu6050.Get_Gyro() / 4;

            

            //得到外环PID输出

            RateError[ROLL] = pid[PIDLEVEL].get_p(errorAngle[ROLL]) - Gyro_ADC.x;

            RateError[PITCH] = pid[PIDLEVEL].get_p(errorAngle[PITCH]) - Gyro_ADC.y;

            RateError[YAW] = ((int32_t)(yawRate) * rc.Command[YAW]) / 32 - Gyro_ADC.z;               

    }

    //飞行器姿态内环控制

    void ANO_FlyControl::Attitude_Inner_Loop(void)

    {

            int32_t PIDTerm[3];

            

            for(u8 i=0; i<3;i++)

            {

                    //当油门低于检查值时积分清零，重新积分  

                    if ((rc.rawData[THROTTLE]) < RC_MINCHECK)        

                            pid[i].reset_I();

                   

                    //得到内环PID输出

                    PIDTerm[i] = pid[i].get_pid(RateError[i], PID_INNER_LOOP_TIME);

            }

            

            PIDTerm[YAW] = -constrain_int32(PIDTerm[YAW], -300 - abs(rc.Command[YAW]), +300 + abs(rc.Command[YAW]));        

                   

            //PID输出转为电机控制量

            motor.writeMotor(rc.Command[THROTTLE], PIDTerm[ROLL], PIDTerm[PITCH], PIDTerm[YAW]);

    }        

//四轴X模式

        motorPWM[0] = throttle - pidTermRoll + pidTermPitch - pidTermYaw; //后右

        motorPWM[1] = throttle - pidTermRoll - pidTermPitch + pidTermYaw; //前右

        motorPWM[2] = throttle + pidTermRoll + pidTermPitch + pidTermYaw; //后左

        motorPWM[3] = throttle + pidTermRoll - pidTermPitch - pidTermYaw; //前左