File content as of revision 1:d83f8332ebfe:

#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();
        }

    }
}