NazaCAN.cpp

00001 #include "mbed.h"
00002 #include "Naza.h"
00003 #include "drone.h"
00004 #include "math.h"
00005 
00006 #ifndef M_PI
00007 #define M_PI 3.14159265358979323846
00008 #endif
00009 
00010 CAN can(p30, p29); // can_rx2, can_tx2
00011 
00012 static uint8_t can_buf[80];
00013 static int can_count = 0, can_id = 0, can_len = 0;
00014 
00015 extern struct GroundData send_data;
00016 extern struct Status stat;
00017 extern DigitalOut led3;
00018 
00019 void parseCan (int id, int len) {
00020     int i;
00021 
00022     switch (id) {
00023     case 0x1003: // gps
00024         if (len != 58) break;
00025         struct Naza_Gps *gps = (struct Naza_Gps *)can_buf;
00026 
00027         memcpy(&send_data.gps, can_buf, len);
00028         if (gps->mask != 0) break; // mask
00029 
00030         if (gps->datetime) {
00031             stat.gps_date = 20000000 + ((gps->datetime >> 25) & 0x3f) * 10000 + ((gps->datetime >> 21) & 0x0f) * 100 + ((gps->datetime >> 16) & 0x1f);
00032             stat.gps_time = ((gps->datetime >> 12) & 0x0f) * 10000 + ((gps->datetime >> 6) & 0x3f) * 100 + (gps->datetime & 0x3f);
00033         }
00034 
00035         stat.gps_lng = gps->longitude;
00036         stat.gps_lat = gps->latitude;
00037         stat.gps_h    = gps->altitude;
00038         stat.gps_sat  = gps->satellites;
00039         stat.gps_type = gps->fix_type;
00040         stat.gps_flg  = gps->flags;
00041 
00042         if (stat.gps_lng && (stat.gps_lng < 1225601000 || stat.gps_lng > 1535911000)) { // yonaguni , minamitori
00043             stat.gps_lng = 0;
00044             stat.gps_lost ++;
00045         }
00046         if (stat.gps_lat && (stat.gps_lat <  202531000 || stat.gps_lat >  453326000)) { // okinotori , etorofu
00047             stat.gps_lat = 0;
00048             stat.gps_lost ++;
00049         }
00050 
00051         if (stat.gps_type == 2 || stat.gps_type == 3) {
00052             stat.gps_lost = 0;
00053         } else {
00054             stat.gps_lost ++;
00055         }
00056 //    printf("%08d %06d lat=%9d lng=%10d h=%4d sat=%d type=%d flg=%d\r\n",
00057 //        stat.gps_date, stat.gps_time, stat.gps_lat, stat.gps_lng, stat.gps_h, stat.gps_sat, stat.gps_type, stat.gps_flg);
00058         break;
00059 
00060     case 0x1004: // compass
00061         if (len != 6) break;
00062         struct Naza_Compass *compass = (struct Naza_Compass *)can_buf;
00063 
00064         memcpy(&send_data.compass, can_buf, len);
00065         stat.compass_x = compass->x;
00066         stat.compass_y = compass->y;
00067         stat.compass_z = compass->z;
00068         float mg = sqrtf(stat.compass_x * stat.compass_x + stat.compass_y * stat.compass_y);
00069         stat.compass   = atanf((stat.compass_y / mg) / (stat.compass_x / mg)) * 180.0 / M_PI;
00070         break;
00071 
00072     case 0x0921: // gps version
00073     case 0x0922: // pmu heart beat
00074         break;
00075 
00076     default:
00077             printf("parse id=%04x %d, ", id, len);
00078             for (i = 0; i < can_len; i ++) {
00079                 printf("%02x ", can_buf[i]);
00080             }
00081             printf("\r\n");
00082         break;
00083     }
00084 }
00085 
00086 void isrCan () {
00087     int i;
00088     CANMessage msg;
00089 
00090     __disable_irq();
00091     if (can.read(msg)) {
00092         __enable_irq();
00093         // can
00094         switch (msg.id) {
00095         case 0x07f8: // gps
00096         case 0x0118: // compass
00097         case 0x01fe: // battery
00098             if (can_id == 0 && msg.data[0] == 0x55 && msg.data[1] == 0xaa && msg.data[2] == 0x55 && msg.data[3] == 0xaa) {
00099                 // header
00100                 can_len = (msg.data[7] << 8) | msg.data[6];
00101                 if (can_len < 0 || can_len >= sizeof(can_buf)) break;
00102                 can_id  = (msg.data[5] << 8) | msg.data[4];
00103                 can_count = 0;
00104             } else
00105             if (can_id) {
00106                 // payload
00107                 for (i = 0; i < msg.len; i ++) {
00108                     if (can_count < sizeof(can_buf)) {
00109                         can_buf[can_count] = msg.data[i];
00110                     }
00111                     can_count ++;
00112                 }
00113                 if (can_count >= can_len + 4) { // + footer
00114                     // done
00115                     if (can_buf[can_len] == 0x66 && can_buf[can_len + 1] == 0xcc && can_buf[can_len + 2] == 0x66 && can_buf[can_len + 3] == 0xcc) {
00116                         parseCan(can_id, can_len);
00117                         led3 = !led3;
00118                     }
00119                     can_id = 0;
00120                 }
00121             }
00122             break;
00123 
00124         default:
00125             printf("CAN id=%04x, ", msg.id);
00126             for (i = 0; i < msg.len; i ++) {
00127                 printf("%02x ", msg.data[i]);
00128             }
00129             printf("\r\n");
00130             break;
00131         }
00132     }
00133     __enable_irq();
00134 }
00135 
00136 void initCan () {
00137     can.frequency(1000000);
00138     can.attach(&isrCan, CAN::RxIrq);
00139 }