Suga koubou / Mbed 2 deprecated DjiNaza

DJI NAZA Controller (Phantom, Futaba S.Bus, CAN bus) see: https://developer.mbed.org/users/okini3939/notebook/drone/

Dependencies:   mbed

DJI NAZA コントローラー

DJI社のフライトコントローラーNAZAシリーズを S.Bus経由でコントロールし、 同時にCANバスからGPSデータを得ることができます。

ラジコンプロポの代わりとして働かせることができます。

see: /users/okini3939/notebook/drone/

main.cpp@0:57f6fbf7380a, 2016-02-08 (annotated)

Committer:
okini3939
Date:
Mon Feb 08 08:53:02 2016 +0000
Revision:
0:57f6fbf7380a
1st build;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
okini3939 0:57f6fbf7380a 1 /*
okini3939 0:57f6fbf7380a 2 * DJI NAZA Controller (Phantom, Futaba S.Bus, CAN bus)
okini3939 0:57f6fbf7380a 3 */
okini3939 0:57f6fbf7380a 4 #include "mbed.h"
okini3939 0:57f6fbf7380a 5 #include "FutabaSBUS.h"
okini3939 0:57f6fbf7380a 6
okini3939 0:57f6fbf7380a 7 #define STICK_CENTER 0x400
okini3939 0:57f6fbf7380a 8 #define STICK_TILT 0x294
okini3939 0:57f6fbf7380a 9 #define STICK_SWITCH 0x200
okini3939 0:57f6fbf7380a 10
okini3939 0:57f6fbf7380a 11 DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
okini3939 0:57f6fbf7380a 12 Serial pc(USBTX, USBRX);
okini3939 0:57f6fbf7380a 13
okini3939 0:57f6fbf7380a 14 FutabaSBUS sbus(p28, p27); // txd2, rxd2
okini3939 0:57f6fbf7380a 15 CAN can(p30, p29); // can_rx2, can_tx2
okini3939 0:57f6fbf7380a 16 DigitalIn sw(p14), sw2(p13), btn(p10);
okini3939 0:57f6fbf7380a 17 DigitalOut sw_pwr1(p12), sw_pwr2(p11);
okini3939 0:57f6fbf7380a 18 AnalogIn adc[6] = {p15, p16, p17, p18, p19, p20};
okini3939 0:57f6fbf7380a 19
okini3939 0:57f6fbf7380a 20 uint8_t can_buf[80];
okini3939 0:57f6fbf7380a 21 int can_count = 0, can_id = 0, can_len = 0;
okini3939 0:57f6fbf7380a 22 int gps_date = 0, gps_time = 0, gps_lat = 0, gps_lng = 0, gps_h = 0, gps_cnt = 0, gps_flg = 0;
okini3939 0:57f6fbf7380a 23 int compass_x = 0, compass_y = 0, compass = 0;
okini3939 0:57f6fbf7380a 24
okini3939 0:57f6fbf7380a 25 void parseCan (int id, int len) {
okini3939 0:57f6fbf7380a 26 int i;
okini3939 0:57f6fbf7380a 27
okini3939 0:57f6fbf7380a 28 switch (id) {
okini3939 0:57f6fbf7380a 29 case 0x1003:
okini3939 0:57f6fbf7380a 30 if (len != 58) break;
okini3939 0:57f6fbf7380a 31 if (can_buf[55] != 0) break; // mask
okini3939 0:57f6fbf7380a 32
okini3939 0:57f6fbf7380a 33 i = (can_buf[ 3] << 24) | (can_buf[ 2] << 16) | (can_buf[ 1] << 8) | can_buf[ 0];
okini3939 0:57f6fbf7380a 34 gps_date = ((i >> 25) & 0x3f) * 10000 + ((i >> 21) & 0x0f) * 100 + ((i >> 16) & 0x1f);
okini3939 0:57f6fbf7380a 35 gps_time = ((i >> 12) & 0x0f) * 10000 + ((i >> 6) & 0x3f) * 100 + (i & 0x3f);
okini3939 0:57f6fbf7380a 36 gps_lat = (can_buf[ 7] << 24) | (can_buf[ 6] << 16) | (can_buf[ 5] << 8) | can_buf[ 4];
okini3939 0:57f6fbf7380a 37 gps_lng = (can_buf[11] << 24) | (can_buf[10] << 16) | (can_buf[ 9] << 8) | can_buf[ 8];
okini3939 0:57f6fbf7380a 38 gps_h = (can_buf[15] << 24) | (can_buf[14] << 16) | (can_buf[13] << 8) | can_buf[12];
okini3939 0:57f6fbf7380a 39 // if (lng != 0 && (lng < 1225601000 || lng > 1535911000)) break; // yonaguni , minamitori
okini3939 0:57f6fbf7380a 40 // if (lat != 0 && (lat < 202531000 || lat > 453326000)) break; // okinotori , etorofu
okini3939 0:57f6fbf7380a 41 gps_cnt = can_buf[49];
okini3939 0:57f6fbf7380a 42 gps_flg = can_buf[51];
okini3939 0:57f6fbf7380a 43 break;
okini3939 0:57f6fbf7380a 44 case 0x1004:
okini3939 0:57f6fbf7380a 45 compass_x = (short)(can_buf[ 1] << 8) | can_buf[ 0];
okini3939 0:57f6fbf7380a 46 compass_y = (short)(can_buf[ 3] << 8) | can_buf[ 2];
okini3939 0:57f6fbf7380a 47 break;
okini3939 0:57f6fbf7380a 48 case 0x0921:
okini3939 0:57f6fbf7380a 49 case 0x0922:
okini3939 0:57f6fbf7380a 50 break;
okini3939 0:57f6fbf7380a 51 default:
okini3939 0:57f6fbf7380a 52 pc.printf("parse id=%04x %d, ", id, len);
okini3939 0:57f6fbf7380a 53 for (i = 0; i < can_len; i ++) {
okini3939 0:57f6fbf7380a 54 pc.printf("%02x ", can_buf[i]);
okini3939 0:57f6fbf7380a 55 }
okini3939 0:57f6fbf7380a 56 pc.printf("\r\n");
okini3939 0:57f6fbf7380a 57 break;
okini3939 0:57f6fbf7380a 58 }
okini3939 0:57f6fbf7380a 59 }
okini3939 0:57f6fbf7380a 60
okini3939 0:57f6fbf7380a 61 void isrCan () {
okini3939 0:57f6fbf7380a 62 int i;
okini3939 0:57f6fbf7380a 63 CANMessage msg;
okini3939 0:57f6fbf7380a 64
okini3939 0:57f6fbf7380a 65 if (can.read(msg)) {
okini3939 0:57f6fbf7380a 66 switch (msg.id) {
okini3939 0:57f6fbf7380a 67 case 0x07f8: // gps
okini3939 0:57f6fbf7380a 68 case 0x0118: // compass
okini3939 0:57f6fbf7380a 69 case 0x01fe: // battery
okini3939 0:57f6fbf7380a 70 if (can_id == 0 && msg.data[0] == 0x55 && msg.data[1] == 0xaa && msg.data[2] == 0x55 && msg.data[3] == 0xaa) {
okini3939 0:57f6fbf7380a 71 can_len = (msg.data[7] << 8) | msg.data[6];
okini3939 0:57f6fbf7380a 72 if (can_len < 0 || can_len >= sizeof(can_buf)) break;
okini3939 0:57f6fbf7380a 73 can_id = (msg.data[5] << 8) | msg.data[4];
okini3939 0:57f6fbf7380a 74 can_count = 0;
okini3939 0:57f6fbf7380a 75 } else
okini3939 0:57f6fbf7380a 76 if (can_id) {
okini3939 0:57f6fbf7380a 77 for (i = 0; i < msg.len; i ++) {
okini3939 0:57f6fbf7380a 78 if (can_count < sizeof(can_buf)) {
okini3939 0:57f6fbf7380a 79 can_buf[can_count] = msg.data[i];
okini3939 0:57f6fbf7380a 80 }
okini3939 0:57f6fbf7380a 81 can_count ++;
okini3939 0:57f6fbf7380a 82 }
okini3939 0:57f6fbf7380a 83 if (can_count >= can_len) {
okini3939 0:57f6fbf7380a 84 parseCan(can_id, can_len);
okini3939 0:57f6fbf7380a 85 can_id = 0;
okini3939 0:57f6fbf7380a 86 }
okini3939 0:57f6fbf7380a 87 }
okini3939 0:57f6fbf7380a 88 break;
okini3939 0:57f6fbf7380a 89
okini3939 0:57f6fbf7380a 90 default:
okini3939 0:57f6fbf7380a 91 pc.printf("CAN id=%04x, ", msg.id);
okini3939 0:57f6fbf7380a 92 for (i = 0; i < msg.len; i ++) {
okini3939 0:57f6fbf7380a 93 pc.printf("%02x ", msg.data[i]);
okini3939 0:57f6fbf7380a 94 }
okini3939 0:57f6fbf7380a 95 pc.printf("\r\n");
okini3939 0:57f6fbf7380a 96 break;
okini3939 0:57f6fbf7380a 97 }
okini3939 0:57f6fbf7380a 98 led3 = !led3;
okini3939 0:57f6fbf7380a 99 }
okini3939 0:57f6fbf7380a 100 }
okini3939 0:57f6fbf7380a 101
okini3939 0:57f6fbf7380a 102 int get_sw () {
okini3939 0:57f6fbf7380a 103 sw_pwr1 = 0;
okini3939 0:57f6fbf7380a 104 sw_pwr2 = 1;
okini3939 0:57f6fbf7380a 105 for (volatile int w = 0; w < 10; w ++);
okini3939 0:57f6fbf7380a 106 if (!sw2) return STICK_CENTER + STICK_SWITCH;
okini3939 0:57f6fbf7380a 107 sw_pwr1 = 1;
okini3939 0:57f6fbf7380a 108 sw_pwr2 = 0;
okini3939 0:57f6fbf7380a 109 for (volatile int w = 0; w < 10; w ++);
okini3939 0:57f6fbf7380a 110 if (!sw2) return STICK_CENTER - STICK_SWITCH;
okini3939 0:57f6fbf7380a 111 return STICK_CENTER;
okini3939 0:57f6fbf7380a 112 }
okini3939 0:57f6fbf7380a 113
okini3939 0:57f6fbf7380a 114 int main() {
okini3939 0:57f6fbf7380a 115 int i, n;
okini3939 0:57f6fbf7380a 116 Timer t, t2;
okini3939 0:57f6fbf7380a 117 CANMessage msg;
okini3939 0:57f6fbf7380a 118
okini3939 0:57f6fbf7380a 119 sw.mode(PullUp);
okini3939 0:57f6fbf7380a 120 sw2.mode(PullUp);
okini3939 0:57f6fbf7380a 121 btn.mode(PullUp);
okini3939 0:57f6fbf7380a 122 pc.baud(115200);
okini3939 0:57f6fbf7380a 123 pc.printf("--- Drone ---\r\n");
okini3939 0:57f6fbf7380a 124
okini3939 0:57f6fbf7380a 125 sbus.failsafe(SBUS_SIGNAL_FAILSAFE);
okini3939 0:57f6fbf7380a 126 sbus.passthrough(false);
okini3939 0:57f6fbf7380a 127
okini3939 0:57f6fbf7380a 128 can.frequency(1000000);
okini3939 0:57f6fbf7380a 129 can.attach(&isrCan, CAN::RxIrq);
okini3939 0:57f6fbf7380a 130
okini3939 0:57f6fbf7380a 131 led1 = 1;
okini3939 0:57f6fbf7380a 132
okini3939 0:57f6fbf7380a 133 t.start();
okini3939 0:57f6fbf7380a 134 t2.start();
okini3939 0:57f6fbf7380a 135 for (;;) {
okini3939 0:57f6fbf7380a 136 if (t2.read_ms() >= 500) {
okini3939 0:57f6fbf7380a 137 t2.reset();
okini3939 0:57f6fbf7380a 138 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);
okini3939 0:57f6fbf7380a 139 pc.printf(" x=%d y=%d\r\n", compass_x, compass_y);
okini3939 0:57f6fbf7380a 140 }
okini3939 0:57f6fbf7380a 141
okini3939 0:57f6fbf7380a 142 if (t.read_ms() >= 20) {
okini3939 0:57f6fbf7380a 143 t.reset();
okini3939 0:57f6fbf7380a 144
okini3939 0:57f6fbf7380a 145 for (i = 0; i < 6; i ++) {
okini3939 0:57f6fbf7380a 146 // 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)
okini3939 0:57f6fbf7380a 147 n = STICK_CENTER + ((adc[i].read() - 0.5) * 2.0 * STICK_TILT);
okini3939 0:57f6fbf7380a 148 if (!btn && i < 4) {
okini3939 0:57f6fbf7380a 149 n = STICK_CENTER + STICK_TILT;
okini3939 0:57f6fbf7380a 150 }
okini3939 0:57f6fbf7380a 151 sbus.servo(1 + i, n);
okini3939 0:57f6fbf7380a 152 }
okini3939 0:57f6fbf7380a 153 // 7:(mode sw)
okini3939 0:57f6fbf7380a 154 sbus.servo(7, get_sw());
okini3939 0:57f6fbf7380a 155 // flag
okini3939 0:57f6fbf7380a 156 sbus.failsafe(sw ? SBUS_SIGNAL_FAILSAFE : SBUS_SIGNAL_OK);
okini3939 0:57f6fbf7380a 157 led2 = !sw;
okini3939 0:57f6fbf7380a 158 }
okini3939 0:57f6fbf7380a 159 }
okini3939 0:57f6fbf7380a 160 }