Diff: usbmsd.cpp

Revision:

0:9f11e7a30865

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/usbmsd.cpp	Thu May 19 09:03:44 2016 +0000
@@ -0,0 +1,82 @@
+#include "mbed.h"
+#include "drone.h"
+#include "USBHostMSD.h"
+
+Thread *thread_msd;
+volatile int log_flg = 0;
+char log_buf[256];
+
+extern struct AirData send_data;
+extern struct Status stat, stat_gnd;
+
+void log () {
+    time_t sec = time(NULL);
+    struct tm *t = localtime(&sec);
+
+    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);
+
+    snprintf(&log_buf[15], sizeof(log_buf) - 15, ",G,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",
+      stat_gnd.gps_lat, stat_gnd.gps_lng, stat_gnd.gps_h, stat_gnd.gps_sat, stat_gnd.gps_type,
+      send_data.aileron, send_data.elevator, send_data.throttle, send_data.rudder,
+      send_data.analog1, send_data.analog2, send_data.sw1, send_data.sw2
+      );
+    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",
+      stat.gps_lat, stat.gps_lng, stat.gps_h,
+      stat.gps_sat, stat.gps_type, stat.gps_flg, stat.gps_lost,
+      stat.compass_x, stat.compass_y, stat.compass_z, stat.compass,
+      stat.uptime, stat.battery, stat.current, stat.amphour, stat.distance1, stat.distance2
+      );
+
+//    printf(log_buf);
+    log_flg = strlen(log_buf);
+}
+
+void taskMsd (void const *args) {
+    int num;
+    FILE *fp;
+    char buf[20];
+    USBHostMSD msd("usb");
+
+    for (;;) {
+        // usb connect
+        while(! msd.connect()) {
+            Thread::wait(500);
+        }
+
+        num = 1;
+        fp = fopen("/usb/seq.txt", "r");
+        if (fp) {
+            if (fgets(buf, sizeof(buf), fp) != NULL) {
+                num = atoi(buf);
+                if (! num) num = 1;
+            }
+            fclose(fp);
+        }
+        fp = fopen("/usb/seq.txt", "w");
+        if (fp) {
+            fprintf(fp, "%d", num + 1);
+            fclose(fp);
+        }
+        // filename
+        sprintf(buf, "/usb/drone%03d.log", num);
+        printf("log: %s\r\n", buf);
+
+        while (msd.connected()) {
+            if (log_flg) {
+                // log
+                fp = fopen(buf, "a");
+                if (fp) {
+                    fwrite(log_buf, sizeof(char), log_flg, fp);
+                    fclose(fp);
+                }
+                log_flg = 0;
+            }
+
+            Thread::wait(1);
+        }
+    }
+}
+
+int initMsd () {
+    thread_msd = new Thread(taskMsd, NULL, osPriorityNormal, 1024 * 4);
+}