DJI NAZA コントローラー

File content as of revision 0:57f6fbf7380a:

/*
 * 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;
        }
    }
}