ジンバルコントローラーとしてのMultiwiiについて その2
2013.2.8追記 最後に変更箇所追加。
国内でも売っている業者さんが多いMultiwii SEから、ATMega2560搭載のALL IN ONE PRO v2.0に替えて、スケッチとにらめっこしながらいろいろ付けたり、外したりしています。
なんとか、元々GAUI CRANE IIIについてきたスペックは立派そうに見えるサーボを使えるように出来ないかという思いからです。実際GAUI CRANE IIIに付属のサーボ、まともに使えている人はいるのでしょうか。。。使えているようなら、是非ノウハウを教えて頂きたいと思います(>_<)。
(1) #define MEGA _HW_GIMBALを有効にして、サーボのガタつきを抑える。
(2) ジンバルに機械的なガタが多く、ギア比も減速が不十分な為、サーボのスピードを抑えたい。
この二つを目標にアレコレ弄ったのが以下の結果です。
一度目のテストでは、弄った所にミスがあり、ピッチ(ティルト)方向に壮絶な振動が生じ失敗に終わりました。(本記事を書いている段階でバグに気づく。修正済)早く飛ばして効果を確かめたいけど、今の季節、朝は日の出が遅いので、飛ばそうとすると仕事に遅刻するので飛ばせないのが切ない所です。
赤字部分がMultiwii2.1のダウンロードしてきたソースからの変更部分
1.config.h内変更箇所
1-1用途
ジンバルコントローラーとしてのみ使用し、他の用途では使用しない。
/**************** The type of multicopter ****************/
#define GIMBAL
//#define BI
//#define TRI
//#define QUADP
//#define QUADX
//#define Y4
//#define Y6
//#define HEX6
//#define HEX6X
//#define OCTOX8
//#define OCTOFLATP
//#define OCTOFLATX
//#define FLYING_WING
//#define VTAIL4
//#define AIRPLANE
//#define SINGLECOPTER
//#define DUALCOPTER
//#define HELI_120_CCPM
//#define HELI_90_DEG
1-2 I2C通信速度
100KHzから400KHzに変更する。
/********************************** I2C speed *****************************/ //#define I2C_SPEED 100000L #define I2C_SPEED 400000L
1-3ボードの指定
#define CRIUS_AIO_PRO_V1
1-4サーボの動作範囲
ニュートラル、ジンバルゲイン、サーボスピードを設定する。
/***********************Cam Stabilisation***********************/ //#define SERVO_MIX_TILT //#define SERVO_TILT #define TILT_PITCH_MIN 1040 //servo travel min, don't set it below 1020 #define TILT_PITCH_MAX 2000 //servo travel max, max value=2000 #define TILT_PITCH_MIDDLE 1475 //servo neutral value #define TILT_PITCH_PROP 30 //servo proportional (tied to angle) #define TILT_ROLL_MIN 1200 #define TILT_ROLL_MAX 1970 #define TILT_ROLL_MIDDLE 1520 #define TILT_ROLL_PROP -50 #define PITCH_SERVO_SPEED 8 //新規追加 #define ROLL_SERVO_SPEED 16 //新規追加
1-5ジャイロのローパスフィルタ
42Hzに設定
#define MPU6050_LPF_42HZ
1-6 ジンバルサーボ移動平均出力
有効にする
#define MMSERVOGIMBAL #define MMSERVOGIMBALVECTORLENGHT 64
1-7サーボリフレッシュレート
/*********************** Servo Refreshrates ***********************/ /* Default 50Hz Servo refresh rate*/ //#define SERVO_RFR_50H //#define SERVO_RFR_160HZ #define SERVO_RFR_300HZ
1-8ジンバルサーボの信号生成(PWM生成)をハードウエアで行う。
#define MEGA_HW_GIMBAL
2.def.h内変更箇所
2-1不要なセンサーを使用しない
#if defined(CRIUS_AIO_PRO_V1) #define MPU6050 //#define HMC5883 //#define MS561101BA #define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = -X; accADC[PITCH] = -Y; accADC[YAW] = Z;} #define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = Y; gyroADC[PITCH] = -X; gyroADC[YAW] = -Z;} //#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = X; magADC[PITCH] = Y; magADC[YAW] = -Z;} //#define MPU6050_EN_I2C_BYPASS // MAG connected to the AUX I2C bus of MPU6050 #undef INTERNAL_I2C_PULLUPS #endif
3.Output.ino変更箇所
3-1ハードウエアPWMでもサーボ出力の移動平均を有効にする。
for (axis=(SEC_SERVO_FROM-1); axis < SEC_SERVO_TO; axis++) { mediaMobileServoGimbalADCSum[axis] -= mediaMobileServoGimbalADC[axis][mediaMobileServoGimbalIDX]; mediaMobileServoGimbalADC[axis][mediaMobileServoGimbalIDX] = servo[axis]; mediaMobileServoGimbalADCSum[axis] += mediaMobileServoGimbalADC[axis][mediaMobileServoGimbalIDX]; #if defined(PROMINI) || (defined(PROMICRO) && defined(HWPWM6)) atomicServo[axis] = (mediaMobileServoGimbalADCSum[axis] / MMSERVOGIMBALVECTORLENGHT - 1000)>>2; #else atomicServo[axis] = (mediaMobileServoGimbalADCSum[axis] / MMSERVOGIMBALVECTORLENGHT - 1000)<<4; #endif //ここから3行追加 #if defined(MEGA) && defined(MEGA_HW_GIMBAL) servo[axis] = mediaMobileServoGimbalADCSum[axis] / MMSERVOGIMBALVECTORLENGHT; #endif //追加部分ここまで }
3-2一回あたりのサーボ移動量を制限する。
// write HW PWM gimbal servos for the mega (needs to be also implemented to the MMSERVOGIMBAL) #if defined(MEGA) && defined(MEGA_HW_GIMBAL) if ( abs(servo[0]-OCR5C) > TILT_PITCH_SPEED ) servo[0] = constrain(servo[0], OCR5C - TILT_PITCH_SPEED, OCR5C + TILT_PITCH_SPEED); if ( abs(servo[1]-OCR5B) > TILT_ROLL_SPEED ) servo[1] = constrain(servo[1], OCR5B - TILT_ROLL_SPEED, OCR5B + ROLL_SERVO_SPEED); OCR5C = servo[0]; OCR5B = servo[1]; #endif
// init Timer 5 of the mega for hw PWM gimbal servos #if defined(MEGA) && defined(MEGA_HW_GIMBAL) TCCR5A |= (1<<WGM51); // phase correct mode & prescaler to 8 TCCR5A &= ~(1<<WGM50); TCCR5B &= ~(1<<WGM52) & ~(1<<CS50) & ~(1<<CS52); TCCR5B |= (1<<WGM53) | (1<<CS51); #if defined(SERVO_RFR_50HZ) ICR5 |= 16700; // TOP to 16700; #endif #if defined(SERVO_RFR_160HZ) ICR5 |= 6200; // TOP to 6200; #endif #if defined(SERVO_RFR_300HZ) ICR5 |= 3330; // TOP to 3330; #endif OCR5C = TILT_PITCH_MIDDLE; OCR5B = TILT_ROLL_MIDDLE; pinMode(44,OUTPUT); TCCR5A |= (1<<COM5C1); // pin 44 pinMode(45,OUTPUT); TCCR5A |= (1<<COM5B1); // pin 45 #endif