Diff: main.cpp

Revision:

0:9f11e7a30865

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 19 09:03:44 2016 +0000
@@ -0,0 +1,319 @@
+#include "mbed.h"
+#include "drone.h"
+#include "SpiOLED.h"
+#include "EthernetPowerControl.h"
+
+#define STICK_CENTER 0x400
+#define STICK_TILT   0x294
+#define STICK_SWITCH 0x200
+#define STICK_CHATTER 30
+
+DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
+DigitalOut led5(p26), led6(p29), led7(p30);
+Serial pc(USBTX, USBRX);
+AnalogIn adc[6] = {p20, p19, p18, p17, p16, p15};
+DigitalIn sw[3] = {p23, p24, p25};
+DigitalOut sw_pwr1(p21), sw_pwr2(p22);
+SpiOLED oled(p5, p6, p7, p8, SpiOLED::LCD20x2);
+
+volatile int received = 0;
+struct AirData send_data;
+
+struct Status stat, stat_gnd;
+volatile int sw_fil = 0, sw_stat = 0, sw_cmd = 0, sw_xcmd = 0;
+int mode = 0;
+int rf_dual = 0;
+
+
+void recvRf (struct GroundData *recv_data, int rssi) {
+
+    if (recv_data->type != DATA_TYPE_GROUND) return;
+
+    parseGps(recv_data->gps);
+    parseCompass(recv_data->compass);
+    stat.uptime    = recv_data->uptime;
+    stat.battery   = recv_data->battery;
+    stat.current   = recv_data->current;
+    stat.amphour   = recv_data->amphour;
+    stat.distance1 = recv_data->distance1;
+    stat.distance2 = recv_data->distance2;
+
+    received = recv_data->seq;
+    led2 = !led2;
+
+    log();
+}
+
+int getAnalog (int n) {
+    int ad;
+
+    __disable_irq();
+    ad = (adc[n].read() - 0.5) * 2.0 * (STICK_TILT + STICK_CHATTER);
+    __enable_irq();
+    if (ad < - STICK_CHATTER) {
+        ad += STICK_CHATTER;
+    } else
+    if (ad > STICK_CHATTER) {
+        ad -= STICK_CHATTER;
+    } else {
+        ad = 0;
+    }
+
+    if (ad < - STICK_TILT) ad = - STICK_TILT;
+    if (ad > STICK_TILT) ad = STICK_TILT;
+
+    return STICK_CENTER + ad;
+}
+
+int getSwitch () {
+    int i, r = 0;
+
+    sw_pwr1 = 0;
+    sw_pwr2 = 1;
+    for (volatile int w = 0; w < 10; w ++);
+    for (i = 0; i < 3; i ++) {
+        if (!sw[i]) r |= (1<<(i * 2));
+    }
+    sw_pwr1 = 1;
+    sw_pwr2 = 0;
+    for (volatile int w = 0; w < 10; w ++);
+    for (i = 0; i < 3; i ++) {
+        if (!sw[i]) r |= (2<<(i * 2));
+    }
+    return r;
+}
+
+void input () {
+    int a, b, c;
+
+    a = getSwitch();
+    if (a == sw_fil) {
+        b = sw_stat;
+        sw_stat = a;
+        c = (b ^ a) & ~a;
+        b = (b ^ a) & a;
+        if (b) sw_cmd = b;
+        if (c) sw_xcmd = c;
+    }
+    sw_fil = a;
+
+    if (sw_stat & 0x01) {
+        // start (ignition)
+        send_data.aileron  = STICK_CENTER + STICK_TILT;
+        send_data.elevator = STICK_CENTER + STICK_TILT;
+        send_data.throttle = STICK_CENTER + STICK_TILT;
+        send_data.rudder   = STICK_CENTER + STICK_TILT;
+        led5 = 1;
+    } else {
+        // normal
+        send_data.aileron  = getAnalog(0);
+        send_data.elevator = getAnalog(1);
+        send_data.throttle = getAnalog(2);
+        send_data.rudder   = getAnalog(3);
+
+        if (send_data.aileron == STICK_CENTER && send_data.elevator == STICK_CENTER && send_data.rudder == STICK_CENTER) {
+            led6 = 1;
+        } else {
+            led6 = 0;
+        }
+        if (send_data.throttle == STICK_CENTER) {
+            led7 = 1;
+        } else {
+            led7 = 0;
+        }
+    }
+
+    send_data.analog1 = getAnalog(4);
+    send_data.analog2 = getAnalog(5);
+    send_data.sw1 = STICK_CENTER - (sw_stat & 0x04 ? STICK_SWITCH : 0) + (sw_stat & 0x08 ? STICK_SWITCH : 0);
+    send_data.sw2 = STICK_CENTER - (sw_stat & 0x10 ? STICK_SWITCH : 0) + (sw_stat & 0x20 ? STICK_SWITCH : 0);
+//    printf(" A=%03x, E=%03x, T=%03x, R=%03x, X1=%03x, X2=%03x, sw=%02x\r\n", send_data.aileron, send_data.elevator, send_data.throttle, send_data.rudder, send_data.analog1, send_data.analog2, sw);
+}
+
+void display () {
+    if (sw_cmd & 0x02) {
+        mode ++;
+        sw_cmd = 0;
+        oled.cls();
+    }
+
+    switch (mode) {
+    case 0:
+        oled.locate(0, 0);
+        oled.printf("%4.1fV %4.1fA %4dmAh", (float)stat.battery / 1000.0, (float)stat.current / 1000.0, stat.amphour);
+        oled.locate(0, 1);
+        oled.printf("%4d %4d", stat.distance1, stat.distance2);
+        break;
+    case 1: // GPS
+        oled.locate(0, 0);
+        oled.printf("%8.5f %9.5f", (float)stat.gps_lat / 10000000.0, (float)stat.gps_lng / 10000000.0);
+        oled.locate(0, 1);
+        oled.printf("%4d %4d %4d %4d", stat.compass_x, stat.compass_y, stat.compass_z, stat.compass);
+        break;
+    case 2: // GPS
+        oled.locate(0, 0);
+        oled.printf("%4d %4d %4d %4d", stat.compass_x, stat.compass_y, stat.compass_z, stat.compass);
+        oled.locate(0, 1);
+        oled.printf("%4d %08d %06d", stat.gps_h, stat.gps_date, stat.gps_time);
+        break;
+    case 3: // GPS ground
+        oled.locate(0, 0);
+        oled.printf("%8.5f %9.5fG", (float)stat_gnd.gps_lat / 10000000.0, (float)stat_gnd.gps_lng / 10000000.0);
+        oled.locate(0, 1);
+        oled.printf("%4d %08d %06d", stat_gnd.gps_h, stat_gnd.gps_date, stat_gnd.gps_time);
+        break;
+    case 4: // stick
+        oled.locate(0, 0);
+        oled.printf("A%03x E%03x T%03x R%03x", send_data.aileron, send_data.elevator, send_data.throttle, send_data.rudder);
+        oled.locate(0, 1);
+        oled.printf(" %03x  %03x  %03x  %03x", send_data.analog1, send_data.analog2, send_data.sw1, send_data.sw2);
+        break;
+    default:
+        mode = 0;
+        break;
+    }
+
+    oled.locate(19, 0);
+    oled.putc((received & 1) ? (rf_dual == 3 ? '*' : '.') : ' ');
+/*
+    pc.printf("Drone sec=%3d %4.1fV %4.1fA %4dmAh", stat.uptime, (float)stat.battery / 1000.0, (float)stat.current / 1000.0, stat.amphour / 1000);
+    pc.printf("  dis1=%4d  dis2=%4d\r\n", stat.distance1, stat.distance2);
+    pc.printf("  lat=%8.5f  lng=%9.5f", (float)stat.gps_lat / 10000000.0, (float)stat.gps_lng / 10000000.0);
+    pc.printf("  h=%d  %08d  %06d\r\n", stat.gps_h, stat.gps_date, stat.gps_time);
+    pc.printf("  x=%4d  y=4d  z=%4d  deg=%4d", stat.compass_x, stat.compass_y, stat.compass_z, stat.compass);
+    pc.printf("  sat=%d  type=%d  flg=%d\r\n", stat.gps_sat, stat.gps_type, stat.gps_flg);
+*/
+}
+
+void center () {
+    int flg = 1;
+
+    for (;;) {
+        input();
+
+        if (send_data.aileron == STICK_CENTER && send_data.elevator == STICK_CENTER && send_data.rudder == STICK_CENTER &&
+          send_data.throttle == STICK_CENTER && send_data.analog1 == STICK_CENTER && send_data.analog2 == STICK_CENTER) {
+            break;
+        }
+
+        if (flg) {
+            flg = 0;
+            oled.cls();
+            oled.locate(6, 0);
+            oled.printf("STICKS ARE");
+            oled.locate(6, 1);
+            oled.printf("NOT IN CENTER!");
+        }
+
+        oled.locate(0, 0);
+        oled.printf("%4d", send_data.analog2 - STICK_CENTER);
+        oled.locate(0, 1);
+        oled.printf("%4d", send_data.analog1 - STICK_CENTER);
+        led5 = !led5;
+        wait_ms(200);
+    }
+    oled.cls();
+}
+
+void fault () {
+    for (;;) {
+        led5 = !led5;
+        wait_ms(200);
+    }
+}
+
+void adjust () {
+    int i;
+
+    if (stat_gnd.gps_date && stat_gnd.gps_time) {
+        struct tm t;
+        t.tm_year = ((stat_gnd.gps_date / 10000) % 10000) - 1900;
+        t.tm_mon  = ((stat_gnd.gps_date / 100) % 100) - 1;
+        t.tm_mday = (stat_gnd.gps_date % 100);
+        t.tm_hour = (stat_gnd.gps_time / 10000) % 100;
+        t.tm_min  = (stat_gnd.gps_time / 100) % 100;
+        t.tm_sec  = stat_gnd.gps_time % 100;
+        time_t gpstime = mktime(&t) + (9 * 60 * 60);
+
+        time_t s = time(NULL);
+        i = s > gpstime ? s - gpstime : gpstime - s;
+        if (i > 1 && t.tm_sec >= 15 && t.tm_sec < 45 && t.tm_year >= 115 && t.tm_year < 138) {
+            set_time(gpstime);
+            printf("time adjust %d -> %d\r\n", s, gpstime);
+        }
+    }
+}
+
+int main() {
+    int count = 0;
+    Timer t;
+
+    PHY_PowerDown();
+//    Peripheral_PowerDown(LPC1768_PCONP_PCUSB|LPC1768_PCONP_PCENET|LPC1768_PCONP_PCI2S);
+    Peripheral_PowerDown(LPC1768_PCONP_PCENET|LPC1768_PCONP_PCI2S);
+    sw[0].mode(PullUp);
+    sw[1].mode(PullUp);
+    sw[2].mode(PullUp);
+    memset(&stat, 0, sizeof(stat));
+    pc.baud(115200);
+    pc.printf("--- Drone Ground ---\r\n");
+
+    center();
+    oled.printf("Suga koubou - Drone");
+
+    if (initRf()) {
+        oled.cls();
+        oled.printf("*** RF error!");
+        fault();
+    }
+    initGps();
+
+    wait_ms(500);
+    adjust();
+    initMsd();
+    led1 = 1;
+
+    t.start();
+    for (;;) {
+        pollRf();
+
+        if (t.read_ms() >= (rf_dual == 3 ? 100 : 200)) {
+            t.reset();
+
+            // stick
+            input();
+            // send
+            if (sendRf(&send_data)) {
+                led4 = 1;
+                led5 = 1;
+            } else {
+                led4 = 0;
+            }
+            led1 = !led1;
+
+            if (received) {
+                // oled
+                display();
+                led5 = 0;
+                received = 0;
+                count = 0;
+            } else {
+                count ++;
+                if (count > 20) {
+                    // lost
+                    led5 = 1;
+                }
+                if (count % 2 == 0) {
+                    display();
+                }
+                if (count % 10 == 0) {
+                    log();
+                }
+            }
+
+            // clock
+            adjust();
+        }
+
+    }
+}