【Kinetis分享】+一起DIY一个Kinetis四轴飞行器！！

只看该作者 · 2014-12-10 01:23

本帖最后由 zhous123 于 2014-12-10 01:25 编辑

这个项目起飞于14年6月，TED上的一个四轴飞行器的视频深深吸引了我，然而我是一个Kinetis的忠实粉丝，就想尝试一下用Kinetis MK10去做一个四轴飞行器！！！

期间因为很多事情，让这个项目断断续续，但是很高兴我能**下来。

当然，尽管现在也还未能成功起飞，但是最起码，有了新的进展，在这里，我分享一下我的Kinetis四轴心得。

也算是我自己的一个学习记录吧。

首先是main函数：

复制

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运行灯闪烁

        }

    }

}

main函数就是飞控一开始执行的代码，主要的一些外设的初始化工作。比如IIC啦，UART啦，SPI啦等等。

InitSensor();其实就是MPU6050 hmc5883 的初始化函数。代码如下：

复制

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;

}

然后在while（1）里，就是最重要的姿态解算函数了。
imu_get_euler_angle(&angle);就是姿态解算函数，并把得到的结果保存到angle变量里。
代码如下：

复制

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;

}

得到飞控的姿态，就可以做很多事情了。

在四轴飞行器里，我们需要使用姿态来做PID控制
常用的是单级PID算法，高级的就是串级PID。
我在尝试使用串级PID
串级PID的代码如下：

复制

//飞行器姿态外环控制

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

}

分为外环控制和内环控制两部分。

PID算法完成，当然是输出给PWM控制电机了。
代码如下：

复制

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

这样，一个基本能飞的四轴飞控系统就差不多完成了。