Diff: NazaCAN.cpp

Revision:

0:4a37291f07ca

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/NazaCAN.cpp	Thu May 19 08:56:47 2016 +0000
@@ -0,0 +1,139 @@
+#include "mbed.h"
+#include "Naza.h"
+#include "drone.h"
+#include "math.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+CAN can(p30, p29); // can_rx2, can_tx2
+
+static uint8_t can_buf[80];
+static int can_count = 0, can_id = 0, can_len = 0;
+
+extern struct GroundData send_data;
+extern struct Status stat;
+extern DigitalOut led3;
+
+void parseCan (int id, int len) {
+    int i;
+
+    switch (id) {
+    case 0x1003: // gps
+        if (len != 58) break;
+        struct Naza_Gps *gps = (struct Naza_Gps *)can_buf;
+
+        memcpy(&send_data.gps, can_buf, len);
+        if (gps->mask != 0) break; // mask
+
+        if (gps->datetime) {
+            stat.gps_date = 20000000 + ((gps->datetime >> 25) & 0x3f) * 10000 + ((gps->datetime >> 21) & 0x0f) * 100 + ((gps->datetime >> 16) & 0x1f);
+            stat.gps_time = ((gps->datetime >> 12) & 0x0f) * 10000 + ((gps->datetime >> 6) & 0x3f) * 100 + (gps->datetime & 0x3f);
+        }
+
+        stat.gps_lng = gps->longitude;
+        stat.gps_lat = gps->latitude;
+        stat.gps_h    = gps->altitude;
+        stat.gps_sat  = gps->satellites;
+        stat.gps_type = gps->fix_type;
+        stat.gps_flg  = gps->flags;
+
+        if (stat.gps_lng && (stat.gps_lng < 1225601000 || stat.gps_lng > 1535911000)) { // yonaguni , minamitori
+            stat.gps_lng = 0;
+            stat.gps_lost ++;
+        }
+        if (stat.gps_lat && (stat.gps_lat <  202531000 || stat.gps_lat >  453326000)) { // okinotori , etorofu
+            stat.gps_lat = 0;
+            stat.gps_lost ++;
+        }
+
+        if (stat.gps_type == 2 || stat.gps_type == 3) {
+            stat.gps_lost = 0;
+        } else {
+            stat.gps_lost ++;
+        }
+//    printf("%08d %06d lat=%9d lng=%10d h=%4d sat=%d type=%d flg=%d\r\n",
+//        stat.gps_date, stat.gps_time, stat.gps_lat, stat.gps_lng, stat.gps_h, stat.gps_sat, stat.gps_type, stat.gps_flg);
+        break;
+
+    case 0x1004: // compass
+        if (len != 6) break;
+        struct Naza_Compass *compass = (struct Naza_Compass *)can_buf;
+
+        memcpy(&send_data.compass, can_buf, len);
+        stat.compass_x = compass->x;
+        stat.compass_y = compass->y;
+        stat.compass_z = compass->z;
+        float mg = sqrtf(stat.compass_x * stat.compass_x + stat.compass_y * stat.compass_y);
+        stat.compass   = atanf((stat.compass_y / mg) / (stat.compass_x / mg)) * 180.0 / M_PI;
+        break;
+
+    case 0x0921: // gps version
+    case 0x0922: // pmu heart beat
+        break;
+
+    default:
+            printf("parse id=%04x %d, ", id, len);
+            for (i = 0; i < can_len; i ++) {
+                printf("%02x ", can_buf[i]);
+            }
+            printf("\r\n");
+        break;
+    }
+}
+
+void isrCan () {
+    int i;
+    CANMessage msg;
+
+    __disable_irq();
+    if (can.read(msg)) {
+        __enable_irq();
+        // can
+        switch (msg.id) {
+        case 0x07f8: // gps
+        case 0x0118: // compass
+        case 0x01fe: // battery
+            if (can_id == 0 && msg.data[0] == 0x55 && msg.data[1] == 0xaa && msg.data[2] == 0x55 && msg.data[3] == 0xaa) {
+                // header
+                can_len = (msg.data[7] << 8) | msg.data[6];
+                if (can_len < 0 || can_len >= sizeof(can_buf)) break;
+                can_id  = (msg.data[5] << 8) | msg.data[4];
+                can_count = 0;
+            } else
+            if (can_id) {
+                // payload
+                for (i = 0; i < msg.len; i ++) {
+                    if (can_count < sizeof(can_buf)) {
+                        can_buf[can_count] = msg.data[i];
+                    }
+                    can_count ++;
+                }
+                if (can_count >= can_len + 4) { // + footer
+                    // done
+                    if (can_buf[can_len] == 0x66 && can_buf[can_len + 1] == 0xcc && can_buf[can_len + 2] == 0x66 && can_buf[can_len + 3] == 0xcc) {
+                        parseCan(can_id, can_len);
+                        led3 = !led3;
+                    }
+                    can_id = 0;
+                }
+            }
+            break;
+
+        default:
+            printf("CAN id=%04x, ", msg.id);
+            for (i = 0; i < msg.len; i ++) {
+                printf("%02x ", msg.data[i]);
+            }
+            printf("\r\n");
+            break;
+        }
+    }
+    __enable_irq();
+}
+
+void initCan () {
+    can.frequency(1000000);
+    can.attach(&isrCan, CAN::RxIrq);
+}