【Kinetis MK10】一起DIY四轴飞行器

ti

`网上看到一个四轴飞行器的视频深深吸引了我，然而我是一个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
", 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
", (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; //前左

复制代码

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

实物图：

`

伸手等你牵 · 2015-9-20 23:03:16

很详细，，，不错，，很好

chen1658137632 · 2015-11-1 22:02:21

享             谢谢分享    分             分
谢谢    谢谢分享             谢谢分享       享             享
谢谢分谢谢分享             谢谢分享       谢             谢
谢谢分谢谢分享                            谢    谢
谢谢分谢谢分享                      谢                谢
      谢谢谢谢                   分                            分
         谢谢                享                                  享

谢谢分享谢谢分享谢谢分享
   谢谢分享谢谢分享                      谢谢分享
   谢谢分享谢谢分享             谢谢分享
               谢谢分享             谢谢分享谢谢分享谢谢分享
         谢谢分享             谢谢分享       谢             谢
         谢谢分享             谢谢分享       谢             谢
         谢谢分享             谢谢分享       分             分
         谢谢分享             谢谢分享       享             享
         谢谢分享             谢谢分享       谢             谢
         谢谢分享             谢谢分享       谢             谢
谢       谢谢分享             谢谢分享    分             分
谢谢    谢谢分享             谢谢分享       享             享

建武 · 2016-1-21 17:01:22

四轴飞行器基础篇不错的文章

guo8113 · 2016-1-25 22:03:03

不错。支持。。。

lcl312 · 2016-2-28 20:22:46

串级PID不好调吧！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！

ccds2016 · 2016-3-17 21:57:30

有没有从入门到精通？

lilac525 · 2016-3-20 15:49:54

实用！感谢分享~

lilac525 · 2016-3-20 15:50:51

楼主再多配一些图效果更好哈，已收藏~

悠悠的冰灵 · 2016-3-25 20:53:05

感谢楼主分享~

天下无为 · 2016-3-25 22:20:36

收藏，收藏。楼主是在太强大了

jirenwudi · 2016-3-30 17:03:54

牛人啊牛人啊牛人啊牛人啊

shamofeng24 · 2016-4-17 22:42:44

谢谢分享。。。。。

伍月MAY · 2016-5-5 16:02:52

赞一个，真不错

Benj · 2016-7-24 10:55:43

谢谢分享

wangtianyu730 · 2016-7-24 16:55:38

厉害

黄小林 · 2016-12-6 22:56:20

不是飞起来了么，啊哈

zzl · 2017-3-20 13:24:02

不错，感谢分享

彭宽成 · 2017-3-21 11:47:48

好厉害，加油。

王鸿 · 2017-4-16 12:17:45

感谢楼主分享，非常有用

江小南瓜 · 2017-6-8 14:22:20

楼主厉害，学习了

搜索历史

【Kinetis MK10】一起DIY四轴飞行器

评分

相关推荐

43 条评论

精选推荐

最新话题

热门话题

创建小组步骤

站长推荐 /7