一起DIY四轴飞行器

    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; //前左