main.cpp

00001 /*
00002  * DJI NAZA Controller (Phantom, Futaba S.Bus, CAN bus)
00003  */
00004 #include "mbed.h"
00005 #include "FutabaSBUS.h"
00006 
00007 #define STICK_CENTER 0x400
00008 #define STICK_TILT   0x294
00009 #define STICK_SWITCH 0x200
00010 
00011 DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
00012 Serial pc(USBTX, USBRX);
00013 
00014 FutabaSBUS sbus(p28, p27); // txd2, rxd2
00015 CAN can(p30, p29); // can_rx2, can_tx2
00016 DigitalIn sw(p14), sw2(p13), btn(p10);
00017 DigitalOut sw_pwr1(p12), sw_pwr2(p11);
00018 AnalogIn adc[6] = {p15, p16, p17, p18, p19, p20};
00019 
00020 uint8_t can_buf[80];
00021 int can_count = 0, can_id = 0, can_len = 0;
00022 int gps_date = 0, gps_time = 0, gps_lat = 0, gps_lng = 0, gps_h = 0, gps_cnt = 0, gps_flg = 0;
00023 int compass_x = 0, compass_y = 0, compass = 0;
00024 
00025 void parseCan (int id, int len) {
00026     int i;
00027 
00028     switch (id) {
00029     case 0x1003:
00030         if (len != 58) break;
00031         if (can_buf[55] != 0) break; // mask
00032 
00033         i        = (can_buf[ 3] << 24) | (can_buf[ 2] << 16) | (can_buf[ 1] << 8) | can_buf[ 0];
00034         gps_date = ((i >> 25) & 0x3f) * 10000 + ((i >> 21) & 0x0f) * 100 + ((i >> 16) & 0x1f);
00035         gps_time = ((i >> 12) & 0x0f) * 10000 + ((i >> 6) & 0x3f) * 100 + (i & 0x3f);
00036         gps_lat  = (can_buf[ 7] << 24) | (can_buf[ 6] << 16) | (can_buf[ 5] << 8) | can_buf[ 4];
00037         gps_lng  = (can_buf[11] << 24) | (can_buf[10] << 16) | (can_buf[ 9] << 8) | can_buf[ 8];
00038         gps_h    = (can_buf[15] << 24) | (can_buf[14] << 16) | (can_buf[13] << 8) | can_buf[12];
00039 //        if (lng != 0 && (lng < 1225601000 || lng > 1535911000)) break; // yonaguni , minamitori
00040 //        if (lat != 0 && (lat <  202531000 || lat >  453326000)) break; // okinotori , etorofu
00041         gps_cnt  = can_buf[49];
00042         gps_flg  = can_buf[51];
00043         break;
00044     case 0x1004:
00045         compass_x = (short)(can_buf[ 1] << 8) | can_buf[ 0];
00046         compass_y = (short)(can_buf[ 3] << 8) | can_buf[ 2];
00047         break;
00048     case 0x0921:
00049     case 0x0922:
00050         break;
00051     default:
00052             pc.printf("parse id=%04x %d, ", id, len);
00053             for (i = 0; i < can_len; i ++) {
00054                 pc.printf("%02x ", can_buf[i]);
00055             }
00056             pc.printf("\r\n");
00057         break;
00058     }
00059 }
00060 
00061 void isrCan () {
00062     int i;
00063     CANMessage msg;
00064 
00065     if (can.read(msg)) {
00066         switch (msg.id) {
00067         case 0x07f8: // gps
00068         case 0x0118: // compass
00069         case 0x01fe: // battery
00070             if (can_id == 0 && msg.data[0] == 0x55 && msg.data[1] == 0xaa && msg.data[2] == 0x55 && msg.data[3] == 0xaa) {
00071                 can_len = (msg.data[7] << 8) | msg.data[6];
00072                 if (can_len < 0 || can_len >= sizeof(can_buf)) break;
00073                 can_id  = (msg.data[5] << 8) | msg.data[4];
00074                 can_count = 0;
00075             } else
00076             if (can_id) {
00077                 for (i = 0; i < msg.len; i ++) {
00078                     if (can_count < sizeof(can_buf)) {
00079                         can_buf[can_count] = msg.data[i];
00080                     }
00081                     can_count ++;
00082                 }
00083                 if (can_count >= can_len) {
00084                     parseCan(can_id, can_len);
00085                     can_id = 0;
00086                 }
00087             }
00088             break;
00089 
00090         default:
00091             pc.printf("CAN id=%04x, ", msg.id);
00092             for (i = 0; i < msg.len; i ++) {
00093                 pc.printf("%02x ", msg.data[i]);
00094             }
00095             pc.printf("\r\n");
00096             break;
00097         }
00098         led3 = !led3;
00099     }
00100 }
00101 
00102 int get_sw () {
00103     sw_pwr1 = 0;
00104     sw_pwr2 = 1;
00105     for (volatile int w = 0; w < 10; w ++);
00106     if (!sw2) return STICK_CENTER + STICK_SWITCH;
00107     sw_pwr1 = 1;
00108     sw_pwr2 = 0;
00109     for (volatile int w = 0; w < 10; w ++);
00110     if (!sw2) return STICK_CENTER - STICK_SWITCH;
00111     return STICK_CENTER;
00112 }
00113 
00114 int main() {
00115     int i, n;
00116     Timer t, t2;
00117     CANMessage msg;
00118 
00119     sw.mode(PullUp);
00120     sw2.mode(PullUp);
00121     btn.mode(PullUp);
00122     pc.baud(115200);
00123     pc.printf("--- Drone ---\r\n");
00124 
00125     sbus.failsafe(SBUS_SIGNAL_FAILSAFE);
00126     sbus.passthrough(false);
00127 
00128     can.frequency(1000000);
00129     can.attach(&isrCan, CAN::RxIrq);
00130 
00131     led1 = 1;
00132 
00133     t.start();
00134     t2.start();
00135     for (;;) {
00136         if (t2.read_ms() >= 500) {
00137             t2.reset();
00138             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);
00139             pc.printf(" x=%d y=%d\r\n", compass_x, compass_y);
00140         }
00141 
00142         if (t.read_ms() >= 20) {
00143             t.reset();
00144 
00145             for (i = 0; i < 6; i ++) {
00146                 // 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)
00147                 n = STICK_CENTER + ((adc[i].read() - 0.5) * 2.0 * STICK_TILT);
00148                 if (!btn && i < 4) {
00149                     n = STICK_CENTER + STICK_TILT;
00150                 }
00151                 sbus.servo(1 + i, n);
00152             }
00153             // 7:(mode sw)
00154             sbus.servo(7, get_sw());
00155             // flag
00156             sbus.failsafe(sw ? SBUS_SIGNAL_FAILSAFE : SBUS_SIGNAL_OK);
00157             led2 = !sw;
00158         }
00159     }
00160 }