DJI Phantom controller hack
DJI PhantomをPCから操作するためにコントローラをhackする。
なお、hackは自己責任で。
コントローラの分解
Phantomのコントローラの裏側のネジを外すと、中身はこんな感じ。
Phantomのコントローラでは、ポテンションメータを使って、操作レバーがどの方向にどれだけ倒されたか検知している。
上下方向に動く操作レバーに付いているポテンションメータ(コントローラ右側)
左右方向に動く操作レバーに付いているポテンションメータ(コントローラ右側)
つまり、操作レバーを倒したときにポテンションメータに加わっている電圧をあらかじめ測定しておき、PCから制御できるマイコンボードを使ってそれと同じだけの電圧をコントローラの制御基板に加えることで、PCからPhantomを操作できるようになるはず。 (電圧制御ではなく、デジタルポテンショメータに置き換える方法もある。)
電圧の確認
ということで、操作レバーを動かした時の電圧をテスタを使って測定する。
まず、テスタで導通チェックすると、2つのポテンションメータにつながっているコードの中で茶・黒のコードがGNDとつながっているみたい。
そして、コントローラの電源を入れた状態で、右側操作レバーを上下左右に倒したときの電圧変化を測定した。 測定値は以下の通り。個体によってばらつきがあるかも。
[上下方向]
橙-赤:1.42V(下)- 1.68V(中)- 1.97V(上)
橙-茶:3.35V(固定)
赤-茶:1.93V(下)- 1.66V(中)- 1.38V(上)
[左右方向]
黄-白:1.45V(左)- 1.72V(中)- 1.99V(右)
黄-黒:3.35V(固定)
白-黒:1.90V(左)- 1.62V(中)- 1.36V(右)
どうやら、橙・黄には固定の電圧がかかっていて、赤・白のコードに加える電圧を制御すれば良いみたい。
回路とプログラムの作成
そして、測定した電圧と同じだけの電圧をマイコンボードからPhantomの制御基板に直接加える。今回はArduino Unoを使用した。
ArduinoのanalogWrite関数を用いることで0-5Vの任意の電圧を作り出すことができる。しかし、この出力はPWM信号であるため、平滑化回路を挟み、アナログ電圧に変換する必要がある(PWM信号のままではうまくいかなかった)。
こちらのサイトを参考に、抵抗(5.1KΩ)、コンデンサ(10uF)、LM358を用いて平滑化のための回路を作成した。
配線は以下の通り。
茶 -> GND
赤 <- 平滑化回路 <- Arduino 5番ピン
橙 <- 3.3V (Arduino)
黒 -> GND
白 <- 平滑化回路 <- Arduino 6番ピン
黄 <- 3.3V (Arduino)
最終的にはこんな感じに配線。コントローラの電源としてAC電源から5Vを入力。
Arduinoのコードは以下の通り。 2・3行目に計測した電圧値を記述してある。
PCの1キーを押すと右側操作レバーが右上に、2キーで初期状態、3キーで左下に倒したのと同じ状態になる。
const int analogOut[] = {5, 6}; // ver, hori int verValues[] = {1.38 * 100, 1.66 * 100, 1.93 * 100}; // up to down int horValues[] = {1.36 * 100, 1.62 * 100, 1.90 * 100}; // right to left int value[] = {0, 0}; // ver, hori, 0-255 void setup() { for (int i = 0; i < sizeof(analogOut) / sizeof(analogOut[0]); i++ ){ pinMode(analogOut[i], OUTPUT); } value[0] = map(verValues[1], 0, 5 * 100, 0, 255); value[1] = map(horValues[1], 0, 5 * 100, 0, 255); Serial.begin(9600); } void loop() { for (int i = 0; i < sizeof(analogOut) / sizeof(analogOut[0]); i++) { analogWrite(analogOut[i], value[i]); } } void serialEvent() { if (Serial.available() > 0) { int key = Serial.read(); switch (key) { case '1': // u-d: 1.38v, l-r: 1.36v value[0] = map(verValues[0], 0, 5 * 100, 0, 255); value[1] = map(horValues[0], 0, 5 * 100, 0, 255); break; case '2': // u-d: 1.66v, l-r: 1.62v value[0] = map(verValues[1], 0, 5 * 100, 0, 255); value[1] = map(horValues[1], 0, 5 * 100, 0, 255); break; case '3': // u-d: 1.93v, l-r: 1.90 value[0] = map(verValues[2], 0, 5 * 100, 0, 255); value[1] = map(horValues[2], 0, 5 * 100, 0, 255); break; case '0': value[0] = 0; value[1] = 0; break; case 'q': verValues[1] += 1; value[0] = map(verValues[1], 0, 5 * 100, 0, 255); Serial.println(verValues[1], DEC); break; case 'w': verValues[1] -= 1; value[0] = map(verValues[1], 0, 5 * 100, 0, 255); Serial.println(verValues[1], DEC); break; case 'e': horValues[1] += 1; value[1] = map(horValues[1], 0, 5 * 100, 0, 255); Serial.println(horValues[1], DEC); break; case 'r': horValues[2] -= 1; value[1] = map(horValues[1], 0, 5 * 100, 0, 255); Serial.println(horValues[1], DEC); break; } for (int i = 0; i < sizeof(value) / sizeof(value[0]); i++) { Serial.print("value["); Serial.print(i); Serial.print("] = "); Serial.println(value[i], DEC); } Serial.println("-------------------"); } }
動作確認
Phantomは左右の操作レバーを内側に倒すと離陸・緊急着陸する。 右側操作レバーはPCから、左側操作レバーは手で操作し、離着陸のテストを行ってみた。
以下、動作映像。
注意
Phantomとコントローラの電源を入れた際に、Phantomのランプがパパパッっと連続して3回赤く光る状態が続く場合がある。操作レバーを倒している状態で電源を入れたときにこうなるみたい。 そのときは、操作レバーを動かしていない(初期状態)ときの電圧値と、プログラムに記述されている電圧値が一致しているか確認する。
追記(2014/5/3)
両方の操作レバーをPCから制御するコードは以下の通り。
const int LEFT_LEVER_OUT[] = {3, 9}; // ver, hori const int RIGHT_LEVER_OUT[] = {10, 11}; // ver, hori const int MAX_VALUE = 5 * 100; int leftVerValues[] = {1.41 * 100, 1.69 * 100, 1.96 * 100}; // up to down int leftHorValues[] = {1.38 * 100, 1.65 * 100, 1.92 * 100}; // right to left int rightVerValues[] = {1.38 * 100, 1.66 * 100, 1.93 * 100}; // up to down int rightHorValues[] = {1.36 * 100, 1.62 * 100, 1.90 * 100}; // right to left int leftValues[] = {0, 0}; // ver, hori, 0-255 int rightValues[] = {0, 0}; // ver, hori, 0-255 boolean isFlying = false; void setup() { for (int i = 0; i < sizeof(LEFT_LEVER_OUT) / sizeof(LEFT_LEVER_OUT[0]); i++) { pinMode(LEFT_LEVER_OUT[i], OUTPUT); } for (int i = 0; i < sizeof(RIGHT_LEVER_OUT) / sizeof(RIGHT_LEVER_OUT[0]); i++) { pinMode(RIGHT_LEVER_OUT[i], OUTPUT); } setLeverValue(leftVerValues[1], leftHorValues[1], leftValues); setLeverValue(rightVerValues[1], rightHorValues[1], rightValues); Serial.begin(9600); } void loop() { for (int i = 0; i < sizeof(LEFT_LEVER_OUT) / sizeof(LEFT_LEVER_OUT[0]); i++) { analogWrite(LEFT_LEVER_OUT[i], leftValues[i]); } for (int i = 0; i < sizeof(RIGHT_LEVER_OUT) / sizeof(RIGHT_LEVER_OUT[0]); i++) { analogWrite(RIGHT_LEVER_OUT[i], rightValues[i]); } } void serialEvent() { if (Serial.available() > 0) { int key = Serial.read(); switch (key) { case '0': // initial state setLeverValue(leftVerValues[1], leftHorValues[1], leftValues); setLeverValue(rightVerValues[1], rightHorValues[1], rightValues); break; case' ': // take off / landing setLeverValue(leftVerValues[0], leftHorValues[0], leftValues); setLeverValue(rightVerValues[2], rightHorValues[2], rightValues); isFlying = !isFlying; break; } for (int i = 0; i < sizeof(leftValues) / sizeof(leftValues[0]); i++) { Serial.print("leftValues["); Serial.print(i); Serial.print("] = "); Serial.println(leftValues[i], DEC); } for (int i = 0; i < sizeof(rightValues) / sizeof(rightValues[0]); i++) { Serial.print("rightValues["); Serial.print(i); Serial.print("] = "); Serial.println(rightValues[i], DEC); } Serial.println("-------------------"); } } void setLeverValue(int verValue, int horValue, int leverValues[]) { leverValues[0] = map(verValue, 0, MAX_VALUE, 0, 255); leverValues[1] = map(horValue, 0, MAX_VALUE, 0, 255); }