Suga koubou / Mbed 2 deprecated Drone_air

DJI NAZA-M controller (multi copter side) see: https://developer.mbed.org/users/okini3939/notebook/drone/

Dependencies:   FutabaSBUS NECnfc mbed

NazaCAN.cpp@1:32cd1cf5d5b1, 2016-05-19 (annotated)

Committer:
okini3939
Date:
Thu May 19 08:59:45 2016 +0000
Revision:
1:32cd1cf5d5b1
Parent:
0:4a37291f07ca 
1st build;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
okini3939 0:4a37291f07ca 1 #include "mbed.h"
okini3939 0:4a37291f07ca 2 #include "Naza.h"
okini3939 0:4a37291f07ca 3 #include "drone.h"
okini3939 0:4a37291f07ca 4 #include "math.h"
okini3939 0:4a37291f07ca 5
okini3939 0:4a37291f07ca 6 #ifndef M_PI
okini3939 0:4a37291f07ca 7 #define M_PI 3.14159265358979323846
okini3939 0:4a37291f07ca 8 #endif
okini3939 0:4a37291f07ca 9
okini3939 0:4a37291f07ca 10 CAN can(p30, p29); // can_rx2, can_tx2
okini3939 0:4a37291f07ca 11
okini3939 0:4a37291f07ca 12 static uint8_t can_buf[80];
okini3939 0:4a37291f07ca 13 static int can_count = 0, can_id = 0, can_len = 0;
okini3939 0:4a37291f07ca 14
okini3939 0:4a37291f07ca 15 extern struct GroundData send_data;
okini3939 0:4a37291f07ca 16 extern struct Status stat;
okini3939 0:4a37291f07ca 17 extern DigitalOut led3;
okini3939 0:4a37291f07ca 18
okini3939 0:4a37291f07ca 19 void parseCan (int id, int len) {
okini3939 0:4a37291f07ca 20 int i;
okini3939 0:4a37291f07ca 21
okini3939 0:4a37291f07ca 22 switch (id) {
okini3939 0:4a37291f07ca 23 case 0x1003: // gps
okini3939 0:4a37291f07ca 24 if (len != 58) break;
okini3939 0:4a37291f07ca 25 struct Naza_Gps *gps = (struct Naza_Gps *)can_buf;
okini3939 0:4a37291f07ca 26
okini3939 0:4a37291f07ca 27 memcpy(&send_data.gps, can_buf, len);
okini3939 0:4a37291f07ca 28 if (gps->mask != 0) break; // mask
okini3939 0:4a37291f07ca 29
okini3939 0:4a37291f07ca 30 if (gps->datetime) {
okini3939 0:4a37291f07ca 31 stat.gps_date = 20000000 + ((gps->datetime >> 25) & 0x3f) * 10000 + ((gps->datetime >> 21) & 0x0f) * 100 + ((gps->datetime >> 16) & 0x1f);
okini3939 0:4a37291f07ca 32 stat.gps_time = ((gps->datetime >> 12) & 0x0f) * 10000 + ((gps->datetime >> 6) & 0x3f) * 100 + (gps->datetime & 0x3f);
okini3939 0:4a37291f07ca 33 }
okini3939 0:4a37291f07ca 34
okini3939 0:4a37291f07ca 35 stat.gps_lng = gps->longitude;
okini3939 0:4a37291f07ca 36 stat.gps_lat = gps->latitude;
okini3939 0:4a37291f07ca 37 stat.gps_h = gps->altitude;
okini3939 0:4a37291f07ca 38 stat.gps_sat = gps->satellites;
okini3939 0:4a37291f07ca 39 stat.gps_type = gps->fix_type;
okini3939 0:4a37291f07ca 40 stat.gps_flg = gps->flags;
okini3939 0:4a37291f07ca 41
okini3939 0:4a37291f07ca 42 if (stat.gps_lng && (stat.gps_lng < 1225601000 || stat.gps_lng > 1535911000)) { // yonaguni , minamitori
okini3939 0:4a37291f07ca 43 stat.gps_lng = 0;
okini3939 0:4a37291f07ca 44 stat.gps_lost ++;
okini3939 0:4a37291f07ca 45 }
okini3939 0:4a37291f07ca 46 if (stat.gps_lat && (stat.gps_lat < 202531000 || stat.gps_lat > 453326000)) { // okinotori , etorofu
okini3939 0:4a37291f07ca 47 stat.gps_lat = 0;
okini3939 0:4a37291f07ca 48 stat.gps_lost ++;
okini3939 0:4a37291f07ca 49 }
okini3939 0:4a37291f07ca 50
okini3939 0:4a37291f07ca 51 if (stat.gps_type == 2 || stat.gps_type == 3) {
okini3939 0:4a37291f07ca 52 stat.gps_lost = 0;
okini3939 0:4a37291f07ca 53 } else {
okini3939 0:4a37291f07ca 54 stat.gps_lost ++;
okini3939 0:4a37291f07ca 55 }
okini3939 0:4a37291f07ca 56 // printf("%08d %06d lat=%9d lng=%10d h=%4d sat=%d type=%d flg=%d\r\n",
okini3939 0:4a37291f07ca 57 // stat.gps_date, stat.gps_time, stat.gps_lat, stat.gps_lng, stat.gps_h, stat.gps_sat, stat.gps_type, stat.gps_flg);
okini3939 0:4a37291f07ca 58 break;
okini3939 0:4a37291f07ca 59
okini3939 0:4a37291f07ca 60 case 0x1004: // compass
okini3939 0:4a37291f07ca 61 if (len != 6) break;
okini3939 0:4a37291f07ca 62 struct Naza_Compass *compass = (struct Naza_Compass *)can_buf;
okini3939 0:4a37291f07ca 63
okini3939 0:4a37291f07ca 64 memcpy(&send_data.compass, can_buf, len);
okini3939 0:4a37291f07ca 65 stat.compass_x = compass->x;
okini3939 0:4a37291f07ca 66 stat.compass_y = compass->y;
okini3939 0:4a37291f07ca 67 stat.compass_z = compass->z;
okini3939 0:4a37291f07ca 68 float mg = sqrtf(stat.compass_x * stat.compass_x + stat.compass_y * stat.compass_y);
okini3939 0:4a37291f07ca 69 stat.compass = atanf((stat.compass_y / mg) / (stat.compass_x / mg)) * 180.0 / M_PI;
okini3939 0:4a37291f07ca 70 break;
okini3939 0:4a37291f07ca 71
okini3939 0:4a37291f07ca 72 case 0x0921: // gps version
okini3939 0:4a37291f07ca 73 case 0x0922: // pmu heart beat
okini3939 0:4a37291f07ca 74 break;
okini3939 0:4a37291f07ca 75
okini3939 0:4a37291f07ca 76 default:
okini3939 0:4a37291f07ca 77 printf("parse id=%04x %d, ", id, len);
okini3939 0:4a37291f07ca 78 for (i = 0; i < can_len; i ++) {
okini3939 0:4a37291f07ca 79 printf("%02x ", can_buf[i]);
okini3939 0:4a37291f07ca 80 }
okini3939 0:4a37291f07ca 81 printf("\r\n");
okini3939 0:4a37291f07ca 82 break;
okini3939 0:4a37291f07ca 83 }
okini3939 0:4a37291f07ca 84 }
okini3939 0:4a37291f07ca 85
okini3939 0:4a37291f07ca 86 void isrCan () {
okini3939 0:4a37291f07ca 87 int i;
okini3939 0:4a37291f07ca 88 CANMessage msg;
okini3939 0:4a37291f07ca 89
okini3939 0:4a37291f07ca 90 __disable_irq();
okini3939 0:4a37291f07ca 91 if (can.read(msg)) {
okini3939 0:4a37291f07ca 92 __enable_irq();
okini3939 0:4a37291f07ca 93 // can
okini3939 0:4a37291f07ca 94 switch (msg.id) {
okini3939 0:4a37291f07ca 95 case 0x07f8: // gps
okini3939 0:4a37291f07ca 96 case 0x0118: // compass
okini3939 0:4a37291f07ca 97 case 0x01fe: // battery
okini3939 0:4a37291f07ca 98 if (can_id == 0 && msg.data[0] == 0x55 && msg.data[1] == 0xaa && msg.data[2] == 0x55 && msg.data[3] == 0xaa) {
okini3939 0:4a37291f07ca 99 // header
okini3939 0:4a37291f07ca 100 can_len = (msg.data[7] << 8) | msg.data[6];
okini3939 0:4a37291f07ca 101 if (can_len < 0 || can_len >= sizeof(can_buf)) break;
okini3939 0:4a37291f07ca 102 can_id = (msg.data[5] << 8) | msg.data[4];
okini3939 0:4a37291f07ca 103 can_count = 0;
okini3939 0:4a37291f07ca 104 } else
okini3939 0:4a37291f07ca 105 if (can_id) {
okini3939 0:4a37291f07ca 106 // payload
okini3939 0:4a37291f07ca 107 for (i = 0; i < msg.len; i ++) {
okini3939 0:4a37291f07ca 108 if (can_count < sizeof(can_buf)) {
okini3939 0:4a37291f07ca 109 can_buf[can_count] = msg.data[i];
okini3939 0:4a37291f07ca 110 }
okini3939 0:4a37291f07ca 111 can_count ++;
okini3939 0:4a37291f07ca 112 }
okini3939 0:4a37291f07ca 113 if (can_count >= can_len + 4) { // + footer
okini3939 0:4a37291f07ca 114 // done
okini3939 0:4a37291f07ca 115 if (can_buf[can_len] == 0x66 && can_buf[can_len + 1] == 0xcc && can_buf[can_len + 2] == 0x66 && can_buf[can_len + 3] == 0xcc) {
okini3939 0:4a37291f07ca 116 parseCan(can_id, can_len);
okini3939 0:4a37291f07ca 117 led3 = !led3;
okini3939 0:4a37291f07ca 118 }
okini3939 0:4a37291f07ca 119 can_id = 0;
okini3939 0:4a37291f07ca 120 }
okini3939 0:4a37291f07ca 121 }
okini3939 0:4a37291f07ca 122 break;
okini3939 0:4a37291f07ca 123
okini3939 0:4a37291f07ca 124 default:
okini3939 0:4a37291f07ca 125 printf("CAN id=%04x, ", msg.id);
okini3939 0:4a37291f07ca 126 for (i = 0; i < msg.len; i ++) {
okini3939 0:4a37291f07ca 127 printf("%02x ", msg.data[i]);
okini3939 0:4a37291f07ca 128 }
okini3939 0:4a37291f07ca 129 printf("\r\n");
okini3939 0:4a37291f07ca 130 break;
okini3939 0:4a37291f07ca 131 }
okini3939 0:4a37291f07ca 132 }
okini3939 0:4a37291f07ca 133 __enable_irq();
okini3939 0:4a37291f07ca 134 }
okini3939 0:4a37291f07ca 135
okini3939 0:4a37291f07ca 136 void initCan () {
okini3939 0:4a37291f07ca 137 can.frequency(1000000);
okini3939 0:4a37291f07ca 138 can.attach(&isrCan, CAN::RxIrq);
okini3939 0:4a37291f07ca 139 }