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

rf920.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 "drone.h"
okini3939 0:4a37291f07ca 3 #include "NECnfc.h"
okini3939 0:4a37291f07ca 4
okini3939 0:4a37291f07ca 5 #define RF_CHANNEL 34
okini3939 0:4a37291f07ca 6
okini3939 0:4a37291f07ca 7 NECnfc rf(p13, p14, p15, p16, p17, 38400 * 2, NECnfc::TYPE_920MHz); // tx, rx, reset, wakeup, mode
okini3939 0:4a37291f07ca 8 //cts 12, rts 13
okini3939 0:4a37291f07ca 9
okini3939 0:4a37291f07ca 10 //static int host = NEC_DUMMYID;
okini3939 0:4a37291f07ca 11 const static int host = 0x4b800062;
okini3939 0:4a37291f07ca 12 static int seq = 1;
okini3939 0:4a37291f07ca 13
okini3939 0:4a37291f07ca 14 RawSerial ext(p28, p27); // sub cpu
okini3939 0:4a37291f07ca 15 char recv_buf[NEC_MAXLENGTH + 1];
okini3939 0:4a37291f07ca 16 volatile int recv_mode = 0;
okini3939 0:4a37291f07ca 17 int recv_len = 0, recv_count = 0;
okini3939 0:4a37291f07ca 18
okini3939 0:4a37291f07ca 19 extern struct Status stat;
okini3939 0:4a37291f07ca 20 extern int rf_dual;
okini3939 0:4a37291f07ca 21
okini3939 0:4a37291f07ca 22 void isrRecv () {
okini3939 0:4a37291f07ca 23 char buf[NEC_MAXLENGTH + 1];
okini3939 0:4a37291f07ca 24 int i, len, dest, src;
okini3939 0:4a37291f07ca 25 uint8_t sum = 0;
okini3939 0:4a37291f07ca 26
okini3939 0:4a37291f07ca 27 len = rf.readData(&dest, &src, buf, sizeof(buf));
okini3939 0:4a37291f07ca 28 if (len <= 0) return;
okini3939 0:4a37291f07ca 29
okini3939 0:4a37291f07ca 30 for (i = 0; i < len; i ++) {
okini3939 0:4a37291f07ca 31 sum += (uint8_t)buf[i];
okini3939 0:4a37291f07ca 32 }
okini3939 0:4a37291f07ca 33 if (sum) return; // sum error
okini3939 0:4a37291f07ca 34 if (strncmp(buf, "Suge", 4) != NULL) return;
okini3939 0:4a37291f07ca 35
okini3939 0:4a37291f07ca 36 len --;
okini3939 0:4a37291f07ca 37 buf[len] = 0;
okini3939 0:4a37291f07ca 38 if (host != src) {
okini3939 0:4a37291f07ca 39 // host = src;
okini3939 0:4a37291f07ca 40 printf("new host %08x\r\n", host);
okini3939 0:4a37291f07ca 41 }
okini3939 0:4a37291f07ca 42 // recvRf((struct AirData *)buf, rf.getRssi());
okini3939 0:4a37291f07ca 43 recvRf((struct AirData *)buf, 0);
okini3939 0:4a37291f07ca 44 }
okini3939 0:4a37291f07ca 45
okini3939 0:4a37291f07ca 46 int sendRf (struct GroundData *send_data) {
okini3939 0:4a37291f07ca 47 int i;
okini3939 0:4a37291f07ca 48 uint8_t sum = 0;
okini3939 0:4a37291f07ca 49 char *buf = (char*)send_data;
okini3939 0:4a37291f07ca 50
okini3939 0:4a37291f07ca 51 memcpy(send_data->magic, "Suge", 4);
okini3939 0:4a37291f07ca 52 send_data->type = DATA_TYPE_GROUND;
okini3939 0:4a37291f07ca 53 send_data->seq = seq;
okini3939 0:4a37291f07ca 54 seq ++;
okini3939 0:4a37291f07ca 55 if (seq >= 0x10000) seq = 1;
okini3939 0:4a37291f07ca 56
okini3939 0:4a37291f07ca 57 for (i = 0; i < sizeof(struct GroundData) - 1; i ++) {
okini3939 0:4a37291f07ca 58 sum += (uint8_t)buf[i];
okini3939 0:4a37291f07ca 59 }
okini3939 0:4a37291f07ca 60 send_data->sum = ~sum + 1; // two's complement
okini3939 0:4a37291f07ca 61
okini3939 0:4a37291f07ca 62 if (rf_dual == 3) {
okini3939 0:4a37291f07ca 63 // sub rf
okini3939 0:4a37291f07ca 64 ext.putc(0x0f);
okini3939 0:4a37291f07ca 65 ext.putc(0x5a);
okini3939 0:4a37291f07ca 66 ext.putc(sizeof(struct GroundData));
okini3939 0:4a37291f07ca 67 ext.putc(~sizeof(struct GroundData));
okini3939 0:4a37291f07ca 68 for (i = 0; i < sizeof(struct GroundData); i ++) {
okini3939 0:4a37291f07ca 69 ext.putc(buf[i]);
okini3939 0:4a37291f07ca 70 }
okini3939 0:4a37291f07ca 71 return 0;
okini3939 0:4a37291f07ca 72 } else {
okini3939 0:4a37291f07ca 73 // main rf
okini3939 0:4a37291f07ca 74 return rf.sendData(host, buf, sizeof(struct GroundData));
okini3939 0:4a37291f07ca 75 }
okini3939 0:4a37291f07ca 76 }
okini3939 0:4a37291f07ca 77
okini3939 0:4a37291f07ca 78 // sub cpu
okini3939 0:4a37291f07ca 79 void isrExt () {
okini3939 0:4a37291f07ca 80 char c;
okini3939 0:4a37291f07ca 81 c = ext.getc();
okini3939 0:4a37291f07ca 82
okini3939 0:4a37291f07ca 83 switch (recv_mode) {
okini3939 0:4a37291f07ca 84 case 0:
okini3939 0:4a37291f07ca 85 if (c == 0x0f) {
okini3939 0:4a37291f07ca 86 recv_mode ++;
okini3939 0:4a37291f07ca 87 }
okini3939 0:4a37291f07ca 88 break;
okini3939 0:4a37291f07ca 89 case 1:
okini3939 0:4a37291f07ca 90 if (c == 0x5a) {
okini3939 0:4a37291f07ca 91 recv_mode ++;
okini3939 0:4a37291f07ca 92 } else {
okini3939 0:4a37291f07ca 93 if (c == 0x0f) break;
okini3939 0:4a37291f07ca 94 recv_mode = 0;
okini3939 0:4a37291f07ca 95 }
okini3939 0:4a37291f07ca 96 break;
okini3939 0:4a37291f07ca 97 case 2:
okini3939 0:4a37291f07ca 98 recv_len = c;
okini3939 0:4a37291f07ca 99 recv_mode ++;
okini3939 0:4a37291f07ca 100 break;
okini3939 0:4a37291f07ca 101 case 3:
okini3939 0:4a37291f07ca 102 if (recv_len == (~c & 0xff) && recv_len <= sizeof(recv_buf)) {
okini3939 0:4a37291f07ca 103 recv_count = 0;
okini3939 0:4a37291f07ca 104 recv_mode ++;
okini3939 0:4a37291f07ca 105 } else {
okini3939 0:4a37291f07ca 106 recv_mode = 0;
okini3939 0:4a37291f07ca 107 }
okini3939 0:4a37291f07ca 108 break;
okini3939 0:4a37291f07ca 109 case 4:
okini3939 0:4a37291f07ca 110 recv_buf[recv_count] = c;
okini3939 0:4a37291f07ca 111 recv_count ++;
okini3939 0:4a37291f07ca 112 if (recv_count >= recv_len || recv_count >= sizeof(recv_buf)) {
okini3939 0:4a37291f07ca 113 recv_mode ++;
okini3939 0:4a37291f07ca 114 }
okini3939 0:4a37291f07ca 115 break;
okini3939 0:4a37291f07ca 116 }
okini3939 0:4a37291f07ca 117 }
okini3939 0:4a37291f07ca 118
okini3939 0:4a37291f07ca 119 void pollRf () {
okini3939 0:4a37291f07ca 120 rf.poll();
okini3939 0:4a37291f07ca 121
okini3939 0:4a37291f07ca 122 if (recv_mode >= 5) {
okini3939 0:4a37291f07ca 123 // sub rf, distance
okini3939 0:4a37291f07ca 124 recv_mode = 0;
okini3939 0:4a37291f07ca 125 int *distance = (int*)&recv_buf;
okini3939 0:4a37291f07ca 126 stat.distance1 = distance[0];
okini3939 0:4a37291f07ca 127 stat.distance2 = distance[1];
okini3939 0:4a37291f07ca 128
okini3939 0:4a37291f07ca 129 if (!(rf_dual & 2)) {
okini3939 0:4a37291f07ca 130 ext.printf("\x0f\x5a\x04\xfbSuge");
okini3939 0:4a37291f07ca 131 }
okini3939 0:4a37291f07ca 132 }
okini3939 0:4a37291f07ca 133 }
okini3939 0:4a37291f07ca 134
okini3939 0:4a37291f07ca 135 int initRf () {
okini3939 0:4a37291f07ca 136 ext.baud(115200);
okini3939 0:4a37291f07ca 137 ext.attach(isrExt);
okini3939 0:4a37291f07ca 138
okini3939 0:4a37291f07ca 139 if (rf.setRfConfig(NECnfc::PWR_MAX, RF_CHANNEL, NECnfc::BAUD_50k)) return -1;
okini3939 0:4a37291f07ca 140 rf.attach(&isrRecv);
okini3939 0:4a37291f07ca 141 printf("RF %08x\r\n", rf.getId());
okini3939 0:4a37291f07ca 142
okini3939 0:4a37291f07ca 143 return 0;
okini3939 0:4a37291f07ca 144 }