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