DJI NAZA コントローラー

Diff: main.cpp

Revision:

0:57f6fbf7380a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Feb 08 08:53:02 2016 +0000
@@ -0,0 +1,160 @@
+/*
+ * DJI NAZA Controller (Phantom, Futaba S.Bus, CAN bus)
+ */
+#include "mbed.h"
+#include "FutabaSBUS.h"
+
+#define STICK_CENTER 0x400
+#define STICK_TILT   0x294
+#define STICK_SWITCH 0x200
+
+DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
+Serial pc(USBTX, USBRX);
+
+FutabaSBUS sbus(p28, p27); // txd2, rxd2
+CAN can(p30, p29); // can_rx2, can_tx2
+DigitalIn sw(p14), sw2(p13), btn(p10);
+DigitalOut sw_pwr1(p12), sw_pwr2(p11);
+AnalogIn adc[6] = {p15, p16, p17, p18, p19, p20};
+
+uint8_t can_buf[80];
+int can_count = 0, can_id = 0, can_len = 0;
+int gps_date = 0, gps_time = 0, gps_lat = 0, gps_lng = 0, gps_h = 0, gps_cnt = 0, gps_flg = 0;
+int compass_x = 0, compass_y = 0, compass = 0;
+
+void parseCan (int id, int len) {
+    int i;
+
+    switch (id) {
+    case 0x1003:
+        if (len != 58) break;
+        if (can_buf[55] != 0) break; // mask
+
+        i        = (can_buf[ 3] << 24) | (can_buf[ 2] << 16) | (can_buf[ 1] << 8) | can_buf[ 0];
+        gps_date = ((i >> 25) & 0x3f) * 10000 + ((i >> 21) & 0x0f) * 100 + ((i >> 16) & 0x1f);
+        gps_time = ((i >> 12) & 0x0f) * 10000 + ((i >> 6) & 0x3f) * 100 + (i & 0x3f);
+        gps_lat  = (can_buf[ 7] << 24) | (can_buf[ 6] << 16) | (can_buf[ 5] << 8) | can_buf[ 4];
+        gps_lng  = (can_buf[11] << 24) | (can_buf[10] << 16) | (can_buf[ 9] << 8) | can_buf[ 8];
+        gps_h    = (can_buf[15] << 24) | (can_buf[14] << 16) | (can_buf[13] << 8) | can_buf[12];
+//        if (lng != 0 && (lng < 1225601000 || lng > 1535911000)) break; // yonaguni , minamitori
+//        if (lat != 0 && (lat <  202531000 || lat >  453326000)) break; // okinotori , etorofu
+        gps_cnt  = can_buf[49];
+        gps_flg  = can_buf[51];
+        break;
+    case 0x1004:
+        compass_x = (short)(can_buf[ 1] << 8) | can_buf[ 0];
+        compass_y = (short)(can_buf[ 3] << 8) | can_buf[ 2];
+        break;
+    case 0x0921:
+    case 0x0922:
+        break;
+    default:
+            pc.printf("parse id=%04x %d, ", id, len);
+            for (i = 0; i < can_len; i ++) {
+                pc.printf("%02x ", can_buf[i]);
+            }
+            pc.printf("\r\n");
+        break;
+    }
+}
+
+void isrCan () {
+    int i;
+    CANMessage msg;
+
+    if (can.read(msg)) {
+        switch (msg.id) {
+        case 0x07f8: // gps
+        case 0x0118: // compass
+        case 0x01fe: // battery
+            if (can_id == 0 && msg.data[0] == 0x55 && msg.data[1] == 0xaa && msg.data[2] == 0x55 && msg.data[3] == 0xaa) {
+                can_len = (msg.data[7] << 8) | msg.data[6];
+                if (can_len < 0 || can_len >= sizeof(can_buf)) break;
+                can_id  = (msg.data[5] << 8) | msg.data[4];
+                can_count = 0;
+            } else
+            if (can_id) {
+                for (i = 0; i < msg.len; i ++) {
+                    if (can_count < sizeof(can_buf)) {
+                        can_buf[can_count] = msg.data[i];
+                    }
+                    can_count ++;
+                }
+                if (can_count >= can_len) {
+                    parseCan(can_id, can_len);
+                    can_id = 0;
+                }
+            }
+            break;
+
+        default:
+            pc.printf("CAN id=%04x, ", msg.id);
+            for (i = 0; i < msg.len; i ++) {
+                pc.printf("%02x ", msg.data[i]);
+            }
+            pc.printf("\r\n");
+            break;
+        }
+        led3 = !led3;
+    }
+}
+
+int get_sw () {
+    sw_pwr1 = 0;
+    sw_pwr2 = 1;
+    for (volatile int w = 0; w < 10; w ++);
+    if (!sw2) return STICK_CENTER + STICK_SWITCH;
+    sw_pwr1 = 1;
+    sw_pwr2 = 0;
+    for (volatile int w = 0; w < 10; w ++);
+    if (!sw2) return STICK_CENTER - STICK_SWITCH;
+    return STICK_CENTER;
+}
+
+int main() {
+    int i, n;
+    Timer t, t2;
+    CANMessage msg;
+
+    sw.mode(PullUp);
+    sw2.mode(PullUp);
+    btn.mode(PullUp);
+    pc.baud(115200);
+    pc.printf("--- Drone ---\r\n");
+
+    sbus.failsafe(SBUS_SIGNAL_FAILSAFE);
+    sbus.passthrough(false);
+
+    can.frequency(1000000);
+    can.attach(&isrCan, CAN::RxIrq);
+
+    led1 = 1;
+
+    t.start();
+    t2.start();
+    for (;;) {
+        if (t2.read_ms() >= 500) {
+            t2.reset();
+            pc.printf("%d %d lat=%d lng=%d h=%d cnt=%d flg=%d", gps_date, gps_time, gps_lat, gps_lng, gps_h, gps_cnt, gps_flg);
+            pc.printf(" x=%d y=%d\r\n", compass_x, compass_y);
+        }
+
+        if (t.read_ms() >= 20) {
+            t.reset();
+
+            for (i = 0; i < 6; i ++) {
+                // 1:A(right L-R), 2:E(left D-U), 3:T(right D-U), 4:R(left L-R), 5:(left sw), 6:(right sw)
+                n = STICK_CENTER + ((adc[i].read() - 0.5) * 2.0 * STICK_TILT);
+                if (!btn && i < 4) {
+                    n = STICK_CENTER + STICK_TILT;
+                }
+                sbus.servo(1 + i, n);
+            }
+            // 7:(mode sw)
+            sbus.servo(7, get_sw());
+            // flag
+            sbus.failsafe(sw ? SBUS_SIGNAL_FAILSAFE : SBUS_SIGNAL_OK);
+            led2 = !sw;
+        }
+    }
+}