main.cpp

00001 #include "mbed.h"
00002 #include "drone.h"
00003 #include "EthernetPowerControl.h"
00004 
00005 DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
00006 Serial pc(USBTX, USBRX);
00007 AnalogIn ad_battery(p19), ad_current(p20);
00008 
00009 volatile int received = 0, timeout = 0;
00010 struct GroundData send_data;
00011 struct Status stat;
00012 int rf_dual = 0;
00013 
00014 // radio control
00015 void recvRf (struct AirData *recv_data, int rssi) {
00016 
00017     if (recv_data->type != DATA_TYPE_AIR) return;
00018 
00019     // send S.Bus
00020     setSBus(1, recv_data->aileron);
00021     setSBus(2, recv_data->elevator);
00022     setSBus(3, recv_data->throttle);
00023     setSBus(4, recv_data->rudder);
00024     setSBus(5, recv_data->analog1);
00025     setSBus(6, recv_data->analog2);
00026     setSBus(7, recv_data->sw1);
00027 //    printf("A=%04d E=%04d T=%04d R=%04d M=%04d G=%04d\r\n",
00028 //        recv_data->aileron, recv_data->elevator, recv_data->throttle, recv_data->rudder, recv_data->mode, recv_data->gain);
00029     setFailsafeSBus(recv_data->sw2 <= 0x400 ? 0 : 1);
00030 
00031     if (rf_dual != (recv_data->flags & 0x03)) {
00032         rf_dual = recv_data->flags & 0x03;
00033         printf("rf_dual %d\r\n", rf_dual);
00034     }
00035     received = recv_data->seq;
00036     timeout = 8; // lost time (*250ms)
00037     led2 = !led2;
00038 }
00039 
00040 // lost control
00041 void lost () {
00042     int i;
00043     for (i = 1; i <= 7; i ++) {
00044         setSBus(i, 0x400);
00045     }
00046     led2 = 0;
00047     printf("lost\r\n");
00048 }
00049 
00050 // telemetry
00051 void send () {
00052 
00053     send_data.type = 0;
00054     send_data.uptime    = stat.uptime;
00055     send_data.current   = stat.current;
00056     send_data.battery   = stat.battery;
00057     send_data.amphour   = stat.amphour;
00058     send_data.distance1 = stat.distance1;
00059     send_data.distance2 = stat.distance2;
00060 
00061     if (sendRf(&send_data)) {
00062         led4 = 1;
00063     }
00064 }
00065 
00066 int main() {
00067     int i, count = 0, timeout = 0;
00068     Timer t, uptime;
00069 
00070     PHY_PowerDown();
00071     Peripheral_PowerDown(LPC1768_PCONP_PCUSB|LPC1768_PCONP_PCENET|LPC1768_PCONP_PCI2S);
00072 
00073     LPC_SC->CCLKCFG = 5; // 48MHz
00074     SystemCoreClockUpdate();
00075     LPC_TIM3->PR = (SystemCoreClock / 4 / 1000000) - 1; // us_ticker
00076 
00077     memset(&stat, 0, sizeof(stat));
00078     pc.baud(115200);
00079     pc.printf("--- Drone Air ---\r\n");
00080 
00081     initSBus();
00082     initCan();
00083     wait_ms(200);
00084     if (initRf()) {
00085 //        return -1;
00086         rf_dual = 3;
00087     }
00088 
00089     uptime.start();
00090     t.start();
00091     led1 = 1;
00092     for (;;) {
00093         pollRf();
00094 
00095         if (t.read_ms() >= 250) { // 4Hz
00096             t.reset();
00097 
00098             // adc
00099             __disable_irq();
00100             stat.battery = (ad_battery * 3300 * 72) / 10; // 62k:10k (mV)
00101             i = (ad_current * 3300 * 40) / 30; // 10k:30k (mV)
00102             __enable_irq();
00103             stat.current = (i * 50) / 4; // 4V/50A (mA)
00104             stat.current_sum += stat.current;
00105             stat.current_count += 1;
00106             stat.amphour = stat.current_sum / (3600 * 4); // 0.25s (mAh)
00107 
00108             count ++;
00109 
00110             if (timeout) {
00111                 timeout --;
00112                 if (timeout == 0) {
00113                     // lost
00114                     lost();
00115                 }
00116             }
00117         }
00118 
00119         if ((count >= 2 && (rf_dual == 3 || received)) || count >= 40) {
00120             // send telemetry
00121             stat.uptime = uptime.read();
00122             send();
00123             count = 0;
00124             received = 0;
00125             led1 = !led1;
00126             led4 = 0;
00127         }
00128     }
00129 }