main.cpp

00001 #include "mbed.h"
00002 #include "drone.h"
00003 #include "SpiOLED.h"
00004 #include "EthernetPowerControl.h"
00005 
00006 #define STICK_CENTER 0x400
00007 #define STICK_TILT   0x294
00008 #define STICK_SWITCH 0x200
00009 #define STICK_CHATTER 30
00010 
00011 DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
00012 DigitalOut led5(p26), led6(p29), led7(p30);
00013 Serial pc(USBTX, USBRX);
00014 AnalogIn adc[6] = {p20, p19, p18, p17, p16, p15};
00015 DigitalIn sw[3] = {p23, p24, p25};
00016 DigitalOut sw_pwr1(p21), sw_pwr2(p22);
00017 SpiOLED oled(p5, p6, p7, p8, SpiOLED::LCD20x2);
00018 
00019 volatile int received = 0;
00020 struct AirData send_data;
00021 
00022 struct Status stat, stat_gnd;
00023 volatile int sw_fil = 0, sw_stat = 0, sw_cmd = 0, sw_xcmd = 0;
00024 int mode = 0;
00025 int rf_dual = 0;
00026 
00027 
00028 void recvRf (struct GroundData *recv_data, int rssi) {
00029 
00030     if (recv_data->type != DATA_TYPE_GROUND) return;
00031 
00032     parseGps(recv_data->gps);
00033     parseCompass(recv_data->compass);
00034     stat.uptime    = recv_data->uptime;
00035     stat.battery   = recv_data->battery;
00036     stat.current   = recv_data->current;
00037     stat.amphour   = recv_data->amphour;
00038     stat.distance1 = recv_data->distance1;
00039     stat.distance2 = recv_data->distance2;
00040 
00041     received = recv_data->seq;
00042     led2 = !led2;
00043 
00044     log();
00045 }
00046 
00047 int getAnalog (int n) {
00048     int ad;
00049 
00050     __disable_irq();
00051     ad = (adc[n].read() - 0.5) * 2.0 * (STICK_TILT + STICK_CHATTER);
00052     __enable_irq();
00053     if (ad < - STICK_CHATTER) {
00054         ad += STICK_CHATTER;
00055     } else
00056     if (ad > STICK_CHATTER) {
00057         ad -= STICK_CHATTER;
00058     } else {
00059         ad = 0;
00060     }
00061 
00062     if (ad < - STICK_TILT) ad = - STICK_TILT;
00063     if (ad > STICK_TILT) ad = STICK_TILT;
00064 
00065     return STICK_CENTER + ad;
00066 }
00067 
00068 int getSwitch () {
00069     int i, r = 0;
00070 
00071     sw_pwr1 = 0;
00072     sw_pwr2 = 1;
00073     for (volatile int w = 0; w < 10; w ++);
00074     for (i = 0; i < 3; i ++) {
00075         if (!sw[i]) r |= (1<<(i * 2));
00076     }
00077     sw_pwr1 = 1;
00078     sw_pwr2 = 0;
00079     for (volatile int w = 0; w < 10; w ++);
00080     for (i = 0; i < 3; i ++) {
00081         if (!sw[i]) r |= (2<<(i * 2));
00082     }
00083     return r;
00084 }
00085 
00086 void input () {
00087     int a, b, c;
00088 
00089     a = getSwitch();
00090     if (a == sw_fil) {
00091         b = sw_stat;
00092         sw_stat = a;
00093         c = (b ^ a) & ~a;
00094         b = (b ^ a) & a;
00095         if (b) sw_cmd = b;
00096         if (c) sw_xcmd = c;
00097     }
00098     sw_fil = a;
00099 
00100     if (sw_stat & 0x01) {
00101         // start (ignition)
00102         send_data.aileron  = STICK_CENTER + STICK_TILT;
00103         send_data.elevator = STICK_CENTER + STICK_TILT;
00104         send_data.throttle = STICK_CENTER + STICK_TILT;
00105         send_data.rudder   = STICK_CENTER + STICK_TILT;
00106         led5 = 1;
00107     } else {
00108         // normal
00109         send_data.aileron  = getAnalog(0);
00110         send_data.elevator = getAnalog(1);
00111         send_data.throttle = getAnalog(2);
00112         send_data.rudder   = getAnalog(3);
00113 
00114         if (send_data.aileron == STICK_CENTER && send_data.elevator == STICK_CENTER && send_data.rudder == STICK_CENTER) {
00115             led6 = 1;
00116         } else {
00117             led6 = 0;
00118         }
00119         if (send_data.throttle == STICK_CENTER) {
00120             led7 = 1;
00121         } else {
00122             led7 = 0;
00123         }
00124     }
00125 
00126     send_data.analog1 = getAnalog(4);
00127     send_data.analog2 = getAnalog(5);
00128     send_data.sw1 = STICK_CENTER - (sw_stat & 0x04 ? STICK_SWITCH : 0) + (sw_stat & 0x08 ? STICK_SWITCH : 0);
00129     send_data.sw2 = STICK_CENTER - (sw_stat & 0x10 ? STICK_SWITCH : 0) + (sw_stat & 0x20 ? STICK_SWITCH : 0);
00130 //    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);
00131 }
00132 
00133 void display () {
00134     if (sw_cmd & 0x02) {
00135         mode ++;
00136         sw_cmd = 0;
00137         oled.cls();
00138     }
00139 
00140     switch (mode) {
00141     case 0:
00142         oled.locate(0, 0);
00143         oled.printf("%4.1fV %4.1fA %4dmAh", (float)stat.battery / 1000.0, (float)stat.current / 1000.0, stat.amphour);
00144         oled.locate(0, 1);
00145         oled.printf("%4d %4d", stat.distance1, stat.distance2);
00146         break;
00147     case 1: // GPS
00148         oled.locate(0, 0);
00149         oled.printf("%8.5f %9.5f", (float)stat.gps_lat / 10000000.0, (float)stat.gps_lng / 10000000.0);
00150         oled.locate(0, 1);
00151         oled.printf("%4d %4d %4d %4d", stat.compass_x, stat.compass_y, stat.compass_z, stat.compass);
00152         break;
00153     case 2: // GPS
00154         oled.locate(0, 0);
00155         oled.printf("%4d %4d %4d %4d", stat.compass_x, stat.compass_y, stat.compass_z, stat.compass);
00156         oled.locate(0, 1);
00157         oled.printf("%4d %08d %06d", stat.gps_h, stat.gps_date, stat.gps_time);
00158         break;
00159     case 3: // GPS ground
00160         oled.locate(0, 0);
00161         oled.printf("%8.5f %9.5fG", (float)stat_gnd.gps_lat / 10000000.0, (float)stat_gnd.gps_lng / 10000000.0);
00162         oled.locate(0, 1);
00163         oled.printf("%4d %08d %06d", stat_gnd.gps_h, stat_gnd.gps_date, stat_gnd.gps_time);
00164         break;
00165     case 4: // stick
00166         oled.locate(0, 0);
00167         oled.printf("A%03x E%03x T%03x R%03x", send_data.aileron, send_data.elevator, send_data.throttle, send_data.rudder);
00168         oled.locate(0, 1);
00169         oled.printf(" %03x  %03x  %03x  %03x", send_data.analog1, send_data.analog2, send_data.sw1, send_data.sw2);
00170         break;
00171     default:
00172         mode = 0;
00173         break;
00174     }
00175 
00176     oled.locate(19, 0);
00177     oled.putc((received & 1) ? (rf_dual == 3 ? '*' : '.') : ' ');
00178 /*
00179     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);
00180     pc.printf("  dis1=%4d  dis2=%4d\r\n", stat.distance1, stat.distance2);
00181     pc.printf("  lat=%8.5f  lng=%9.5f", (float)stat.gps_lat / 10000000.0, (float)stat.gps_lng / 10000000.0);
00182     pc.printf("  h=%d  %08d  %06d\r\n", stat.gps_h, stat.gps_date, stat.gps_time);
00183     pc.printf("  x=%4d  y=4d  z=%4d  deg=%4d", stat.compass_x, stat.compass_y, stat.compass_z, stat.compass);
00184     pc.printf("  sat=%d  type=%d  flg=%d\r\n", stat.gps_sat, stat.gps_type, stat.gps_flg);
00185 */
00186 }
00187 
00188 void center () {
00189     int flg = 1;
00190 
00191     for (;;) {
00192         input();
00193 
00194         if (send_data.aileron == STICK_CENTER && send_data.elevator == STICK_CENTER && send_data.rudder == STICK_CENTER &&
00195           send_data.throttle == STICK_CENTER && send_data.analog1 == STICK_CENTER && send_data.analog2 == STICK_CENTER) {
00196             break;
00197         }
00198 
00199         if (flg) {
00200             flg = 0;
00201             oled.cls();
00202             oled.locate(6, 0);
00203             oled.printf("STICKS ARE");
00204             oled.locate(6, 1);
00205             oled.printf("NOT IN CENTER!");
00206         }
00207 
00208         oled.locate(0, 0);
00209         oled.printf("%4d", send_data.analog2 - STICK_CENTER);
00210         oled.locate(0, 1);
00211         oled.printf("%4d", send_data.analog1 - STICK_CENTER);
00212         led5 = !led5;
00213         wait_ms(200);
00214     }
00215     oled.cls();
00216 }
00217 
00218 void fault () {
00219     for (;;) {
00220         led5 = !led5;
00221         wait_ms(200);
00222     }
00223 }
00224 
00225 void adjust () {
00226     int i;
00227 
00228     if (stat_gnd.gps_date && stat_gnd.gps_time) {
00229         struct tm t;
00230         t.tm_year = ((stat_gnd.gps_date / 10000) % 10000) - 1900;
00231         t.tm_mon  = ((stat_gnd.gps_date / 100) % 100) - 1;
00232         t.tm_mday = (stat_gnd.gps_date % 100);
00233         t.tm_hour = (stat_gnd.gps_time / 10000) % 100;
00234         t.tm_min  = (stat_gnd.gps_time / 100) % 100;
00235         t.tm_sec  = stat_gnd.gps_time % 100;
00236         time_t gpstime = mktime(&t) + (9 * 60 * 60);
00237 
00238         time_t s = time(NULL);
00239         i = s > gpstime ? s - gpstime : gpstime - s;
00240         if (i > 1 && t.tm_sec >= 15 && t.tm_sec < 45 && t.tm_year >= 115 && t.tm_year < 138) {
00241             set_time(gpstime);
00242             printf("time adjust %d -> %d\r\n", s, gpstime);
00243         }
00244     }
00245 }
00246 
00247 int main() {
00248     int count = 0;
00249     Timer t;
00250 
00251     PHY_PowerDown();
00252 //    Peripheral_PowerDown(LPC1768_PCONP_PCUSB|LPC1768_PCONP_PCENET|LPC1768_PCONP_PCI2S);
00253     Peripheral_PowerDown(LPC1768_PCONP_PCENET|LPC1768_PCONP_PCI2S);
00254     sw[0].mode(PullUp);
00255     sw[1].mode(PullUp);
00256     sw[2].mode(PullUp);
00257     memset(&stat, 0, sizeof(stat));
00258     pc.baud(115200);
00259     pc.printf("--- Drone Ground ---\r\n");
00260 
00261     center();
00262     oled.printf("Suga koubou - Drone");
00263 
00264     if (initRf()) {
00265         oled.cls();
00266         oled.printf("*** RF error!");
00267         fault();
00268     }
00269     initGps();
00270 
00271     wait_ms(500);
00272     adjust();
00273     initMsd();
00274     led1 = 1;
00275 
00276     t.start();
00277     for (;;) {
00278         pollRf();
00279 
00280         if (t.read_ms() >= (rf_dual == 3 ? 100 : 200)) {
00281             t.reset();
00282 
00283             // stick
00284             input();
00285             // send
00286             if (sendRf(&send_data)) {
00287                 led4 = 1;
00288                 led5 = 1;
00289             } else {
00290                 led4 = 0;
00291             }
00292             led1 = !led1;
00293 
00294             if (received) {
00295                 // oled
00296                 display();
00297                 led5 = 0;
00298                 received = 0;
00299                 count = 0;
00300             } else {
00301                 count ++;
00302                 if (count > 20) {
00303                     // lost
00304                     led5 = 1;
00305                 }
00306                 if (count % 2 == 0) {
00307                     display();
00308                 }
00309                 if (count % 10 == 0) {
00310                     log();
00311                 }
00312             }
00313 
00314             // clock
00315             adjust();
00316         }
00317 
00318     }
00319 }