DJI NAZA-M controller (multi copter side) see: https://developer.mbed.org/users/okini3939/notebook/drone/
Dependencies: FutabaSBUS NECnfc mbed
NazaCAN.cpp
- Committer:
- okini3939
- Date:
- 2016-05-19
- Revision:
- 1:32cd1cf5d5b1
- Parent:
- 0:4a37291f07ca
File content as of revision 1:32cd1cf5d5b1:
#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); }