|
|
本帖最后由 sdu1028 于 2014-5-14 14:56 编辑
因为之前仿过MK的四轴，不过没做过软件。上次五一假期自己写了下飞控算法弥补一下。简单粗暴，直接有效
code全是自己做的，最简单的complementary filter，加RPY的PID控制。事实证明这种简单的算法还是最有效的，其实大部分时间都耗在调参数上。
用的Arduino的板子和GY-88 IMU snesor，还有一个超声测距模块,用来定高。
遥控用串口转蓝牙模块，还写了一个Android App来控制。
上video，还算稳定：
http://v.youku.com/v_show/id_XNzExNTc4ODIw.html
http://v.youku.com/v_show/id_XNzExODU1NTMy.html
附PID控制部分的code:
void loop()
{
dt_us_=micros()-time_us_;
time_us_=micros();
//There's no delays in this sketch while doing distance pings.
if (millis() >= pingTimer) {
pingTimer += pingSpeed; // Set the next ping time.
sonarAltitude.ping_timer(echoCheck); // Send out the ping
}
//update IMU data
IMU_proc();
rotation_x = imu_data[1] - sensor_to_ground_bias[1];
rotation_y = imu_data[2] - sensor_to_ground_bias[2];
rotation_z = imu_data[3] - sensor_to_ground_bias[3];
//Caculate average rotation speed, degrees per 100ms
float drotation_x = 100.0f*(rotation_x - hrotation_x)/(((float)dt_us_)/1000.0f);
float drotation_y = 100.0f*(rotation_y - hrotation_y)/(((float)dt_us_)/1000.0f);
float drotation_z = 100.0f*(rotation_z - hrotation_z)/(((float)dt_us_)/1000.0f);
//serialPrintRotation();
//printserialChart();
getSerial3Cmd();
//PD Balance Control
adjustAC = - ( P_ * (rotation_x - desired_pitch_x) - D_ * drotation_x );
adjustBD = - ( P_ * (rotation_y - desired_pitch_y) - D_ * drotation_y );
//PD Yaw control
desired_heading = hrotation_z;
heading_integ_value = rotation_z-lock_yaw;// + I_HEADING_*(rotation_z -desired_heading);
float heading_trim = heading_integ_value*P_HEADING_ - drotation_z* D_HEADING_;
heading_trim = limitAbs(heading_trim, HEADING_TREIM_LIMIT);
//PID controller for altitude
float altitude_trim = - P_ALT_T_ * (sonar_altitude_cm_ - desired_altitude) - D_ALT_T_*daltitude_cm_;
altitude_trim = limitAbs(altitude_trim, MAX_ALTI_TRIM );
//limit max speed
speeda=limitMAXSPD(upspeed + adjustAC + heading_trim + altitude_trim);
speedb=limitMAXSPD(upspeed + adjustBD - heading_trim + altitude_trim);
speedc=limitMAXSPD(upspeed - adjustAC + heading_trim + altitude_trim);
speedd=limitMAXSPD(upspeed - adjustBD - heading_trim + altitude_trim);
//update speed
setFormatedSpeed(1, speeda); //a
setFormatedSpeed(2, speedb); //b
setFormatedSpeed(3, speedc); //c
setFormatedSpeed(4, speedd); //d
//Record history rotation
hrotation_x = rotation_x;
hrotation_y = rotation_y;
hrotation_z = rotation_z;
dt_us_=micros()-time_us_;
if(dt_us_ < CONTROL_CYCLE_US)
delayMicroseconds(CONTROL_CYCLE_US-dt_us_); //delay to 5ms ->200Hz
}
|
阿莫论坛20周年了！感谢大家的支持与爱护！！
知道什么是神吗？其实神本来也是人，只不过神做了人做不到的事情 所以才成了神。 （头文字D, 杜汶泽）
|
发表于 2014-5-13 21:52:35
楼主
