File content as of revision 1:32cd1cf5d5b1:

#include "mbed.h"
#include "drone.h"
#include "EthernetPowerControl.h"

DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
Serial pc(USBTX, USBRX);
AnalogIn ad_battery(p19), ad_current(p20);

volatile int received = 0, timeout = 0;
struct GroundData send_data;
struct Status stat;
int rf_dual = 0;

// radio control
void recvRf (struct AirData *recv_data, int rssi) {

    if (recv_data->type != DATA_TYPE_AIR) return;

    // send S.Bus
    setSBus(1, recv_data->aileron);
    setSBus(2, recv_data->elevator);
    setSBus(3, recv_data->throttle);
    setSBus(4, recv_data->rudder);
    setSBus(5, recv_data->analog1);
    setSBus(6, recv_data->analog2);
    setSBus(7, recv_data->sw1);
//    printf("A=%04d E=%04d T=%04d R=%04d M=%04d G=%04d\r\n",
//        recv_data->aileron, recv_data->elevator, recv_data->throttle, recv_data->rudder, recv_data->mode, recv_data->gain);
    setFailsafeSBus(recv_data->sw2 <= 0x400 ? 0 : 1);

    if (rf_dual != (recv_data->flags & 0x03)) {
        rf_dual = recv_data->flags & 0x03;
        printf("rf_dual %d\r\n", rf_dual);
    }
    received = recv_data->seq;
    timeout = 8; // lost time (*250ms)
    led2 = !led2;
}

// lost control
void lost () {
    int i;
    for (i = 1; i <= 7; i ++) {
        setSBus(i, 0x400);
    }
    led2 = 0;
    printf("lost\r\n");
}

// telemetry
void send () {

    send_data.type = 0;
    send_data.uptime    = stat.uptime;
    send_data.current   = stat.current;
    send_data.battery   = stat.battery;
    send_data.amphour   = stat.amphour;
    send_data.distance1 = stat.distance1;
    send_data.distance2 = stat.distance2;

    if (sendRf(&send_data)) {
        led4 = 1;
    }
}

int main() {
    int i, count = 0, timeout = 0;
    Timer t, uptime;

    PHY_PowerDown();
    Peripheral_PowerDown(LPC1768_PCONP_PCUSB|LPC1768_PCONP_PCENET|LPC1768_PCONP_PCI2S);

    LPC_SC->CCLKCFG = 5; // 48MHz
    SystemCoreClockUpdate();
    LPC_TIM3->PR = (SystemCoreClock / 4 / 1000000) - 1; // us_ticker

    memset(&stat, 0, sizeof(stat));
    pc.baud(115200);
    pc.printf("--- Drone Air ---\r\n");

    initSBus();
    initCan();
    wait_ms(200);
    if (initRf()) {
//        return -1;
        rf_dual = 3;
    }

    uptime.start();
    t.start();
    led1 = 1;
    for (;;) {
        pollRf();

        if (t.read_ms() >= 250) { // 4Hz
            t.reset();

            // adc
            __disable_irq();
            stat.battery = (ad_battery * 3300 * 72) / 10; // 62k:10k (mV)
            i = (ad_current * 3300 * 40) / 30; // 10k:30k (mV)
            __enable_irq();
            stat.current = (i * 50) / 4; // 4V/50A (mA)
            stat.current_sum += stat.current;
            stat.current_count += 1;
            stat.amphour = stat.current_sum / (3600 * 4); // 0.25s (mAh)

            count ++;

            if (timeout) {
                timeout --;
                if (timeout == 0) {
                    // lost
                    lost();
                }
            }
        }

        if ((count >= 2 && (rf_dual == 3 || received)) || count >= 40) {
            // send telemetry
            stat.uptime = uptime.read();
            send();
            count = 0;
            received = 0;
            led1 = !led1;
            led4 = 0;
        }
    }
}