DJI NAZA-M controller (remote controller side) see: https://developer.mbed.org/users/okini3939/notebook/drone/
Dependencies: NECnfc SpiOLED USBHost mbed
usbmsd.cpp@1:d83f8332ebfe, 2016-05-19 (annotated)
- Committer:
- okini3939
- Date:
- Thu May 19 09:05:02 2016 +0000
- Revision:
- 1:d83f8332ebfe
- Parent:
- 0:9f11e7a30865
1st build;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
okini3939 | 0:9f11e7a30865 | 1 | #include "mbed.h" |
okini3939 | 0:9f11e7a30865 | 2 | #include "drone.h" |
okini3939 | 0:9f11e7a30865 | 3 | #include "USBHostMSD.h" |
okini3939 | 0:9f11e7a30865 | 4 | |
okini3939 | 0:9f11e7a30865 | 5 | Thread *thread_msd; |
okini3939 | 0:9f11e7a30865 | 6 | volatile int log_flg = 0; |
okini3939 | 0:9f11e7a30865 | 7 | char log_buf[256]; |
okini3939 | 0:9f11e7a30865 | 8 | |
okini3939 | 0:9f11e7a30865 | 9 | extern struct AirData send_data; |
okini3939 | 0:9f11e7a30865 | 10 | extern struct Status stat, stat_gnd; |
okini3939 | 0:9f11e7a30865 | 11 | |
okini3939 | 0:9f11e7a30865 | 12 | void log () { |
okini3939 | 0:9f11e7a30865 | 13 | time_t sec = time(NULL); |
okini3939 | 0:9f11e7a30865 | 14 | struct tm *t = localtime(&sec); |
okini3939 | 0:9f11e7a30865 | 15 | |
okini3939 | 0:9f11e7a30865 | 16 | snprintf(log_buf, sizeof(log_buf), "%04d%02d%02d,%02d%02d%02d", t->tm_year + 1900, t->tm_mon + 1, t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec); |
okini3939 | 0:9f11e7a30865 | 17 | |
okini3939 | 0:9f11e7a30865 | 18 | snprintf(&log_buf[15], sizeof(log_buf) - 15, ",G,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d", |
okini3939 | 0:9f11e7a30865 | 19 | stat_gnd.gps_lat, stat_gnd.gps_lng, stat_gnd.gps_h, stat_gnd.gps_sat, stat_gnd.gps_type, |
okini3939 | 0:9f11e7a30865 | 20 | send_data.aileron, send_data.elevator, send_data.throttle, send_data.rudder, |
okini3939 | 0:9f11e7a30865 | 21 | send_data.analog1, send_data.analog2, send_data.sw1, send_data.sw2 |
okini3939 | 0:9f11e7a30865 | 22 | ); |
okini3939 | 0:9f11e7a30865 | 23 | snprintf(&log_buf[strlen(log_buf)], sizeof(log_buf) - strlen(log_buf), ",A,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n", |
okini3939 | 0:9f11e7a30865 | 24 | stat.gps_lat, stat.gps_lng, stat.gps_h, |
okini3939 | 0:9f11e7a30865 | 25 | stat.gps_sat, stat.gps_type, stat.gps_flg, stat.gps_lost, |
okini3939 | 0:9f11e7a30865 | 26 | stat.compass_x, stat.compass_y, stat.compass_z, stat.compass, |
okini3939 | 0:9f11e7a30865 | 27 | stat.uptime, stat.battery, stat.current, stat.amphour, stat.distance1, stat.distance2 |
okini3939 | 0:9f11e7a30865 | 28 | ); |
okini3939 | 0:9f11e7a30865 | 29 | |
okini3939 | 0:9f11e7a30865 | 30 | // printf(log_buf); |
okini3939 | 0:9f11e7a30865 | 31 | log_flg = strlen(log_buf); |
okini3939 | 0:9f11e7a30865 | 32 | } |
okini3939 | 0:9f11e7a30865 | 33 | |
okini3939 | 0:9f11e7a30865 | 34 | void taskMsd (void const *args) { |
okini3939 | 0:9f11e7a30865 | 35 | int num; |
okini3939 | 0:9f11e7a30865 | 36 | FILE *fp; |
okini3939 | 0:9f11e7a30865 | 37 | char buf[20]; |
okini3939 | 0:9f11e7a30865 | 38 | USBHostMSD msd("usb"); |
okini3939 | 0:9f11e7a30865 | 39 | |
okini3939 | 0:9f11e7a30865 | 40 | for (;;) { |
okini3939 | 0:9f11e7a30865 | 41 | // usb connect |
okini3939 | 0:9f11e7a30865 | 42 | while(! msd.connect()) { |
okini3939 | 0:9f11e7a30865 | 43 | Thread::wait(500); |
okini3939 | 0:9f11e7a30865 | 44 | } |
okini3939 | 0:9f11e7a30865 | 45 | |
okini3939 | 0:9f11e7a30865 | 46 | num = 1; |
okini3939 | 0:9f11e7a30865 | 47 | fp = fopen("/usb/seq.txt", "r"); |
okini3939 | 0:9f11e7a30865 | 48 | if (fp) { |
okini3939 | 0:9f11e7a30865 | 49 | if (fgets(buf, sizeof(buf), fp) != NULL) { |
okini3939 | 0:9f11e7a30865 | 50 | num = atoi(buf); |
okini3939 | 0:9f11e7a30865 | 51 | if (! num) num = 1; |
okini3939 | 0:9f11e7a30865 | 52 | } |
okini3939 | 0:9f11e7a30865 | 53 | fclose(fp); |
okini3939 | 0:9f11e7a30865 | 54 | } |
okini3939 | 0:9f11e7a30865 | 55 | fp = fopen("/usb/seq.txt", "w"); |
okini3939 | 0:9f11e7a30865 | 56 | if (fp) { |
okini3939 | 0:9f11e7a30865 | 57 | fprintf(fp, "%d", num + 1); |
okini3939 | 0:9f11e7a30865 | 58 | fclose(fp); |
okini3939 | 0:9f11e7a30865 | 59 | } |
okini3939 | 0:9f11e7a30865 | 60 | // filename |
okini3939 | 0:9f11e7a30865 | 61 | sprintf(buf, "/usb/drone%03d.log", num); |
okini3939 | 0:9f11e7a30865 | 62 | printf("log: %s\r\n", buf); |
okini3939 | 0:9f11e7a30865 | 63 | |
okini3939 | 0:9f11e7a30865 | 64 | while (msd.connected()) { |
okini3939 | 0:9f11e7a30865 | 65 | if (log_flg) { |
okini3939 | 0:9f11e7a30865 | 66 | // log |
okini3939 | 0:9f11e7a30865 | 67 | fp = fopen(buf, "a"); |
okini3939 | 0:9f11e7a30865 | 68 | if (fp) { |
okini3939 | 0:9f11e7a30865 | 69 | fwrite(log_buf, sizeof(char), log_flg, fp); |
okini3939 | 0:9f11e7a30865 | 70 | fclose(fp); |
okini3939 | 0:9f11e7a30865 | 71 | } |
okini3939 | 0:9f11e7a30865 | 72 | log_flg = 0; |
okini3939 | 0:9f11e7a30865 | 73 | } |
okini3939 | 0:9f11e7a30865 | 74 | |
okini3939 | 0:9f11e7a30865 | 75 | Thread::wait(1); |
okini3939 | 0:9f11e7a30865 | 76 | } |
okini3939 | 0:9f11e7a30865 | 77 | } |
okini3939 | 0:9f11e7a30865 | 78 | } |
okini3939 | 0:9f11e7a30865 | 79 | |
okini3939 | 0:9f11e7a30865 | 80 | int initMsd () { |
okini3939 | 0:9f11e7a30865 | 81 | thread_msd = new Thread(taskMsd, NULL, osPriorityNormal, 1024 * 4); |
okini3939 | 0:9f11e7a30865 | 82 | } |