Suga koubou / Mbed 2 deprecated Drone_ground

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

Dependencies:   NECnfc SpiOLED USBHost mbed

main.cpp@1:d83f8332ebfe, 2016-05-19 (annotated)

Committer:
okini3939
Date:
Thu May 19 09:05:02 2016 +0000
Revision:
1:d83f8332ebfe
Parent:
0:9f11e7a30865 
1st build;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
okini3939 0:9f11e7a30865 1 #include "mbed.h"
okini3939 0:9f11e7a30865 2 #include "drone.h"
okini3939 0:9f11e7a30865 3 #include "SpiOLED.h"
okini3939 0:9f11e7a30865 4 #include "EthernetPowerControl.h"
okini3939 0:9f11e7a30865 5
okini3939 0:9f11e7a30865 6 #define STICK_CENTER 0x400
okini3939 0:9f11e7a30865 7 #define STICK_TILT 0x294
okini3939 0:9f11e7a30865 8 #define STICK_SWITCH 0x200
okini3939 0:9f11e7a30865 9 #define STICK_CHATTER 30
okini3939 0:9f11e7a30865 10
okini3939 0:9f11e7a30865 11 DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
okini3939 0:9f11e7a30865 12 DigitalOut led5(p26), led6(p29), led7(p30);
okini3939 0:9f11e7a30865 13 Serial pc(USBTX, USBRX);
okini3939 0:9f11e7a30865 14 AnalogIn adc[6] = {p20, p19, p18, p17, p16, p15};
okini3939 0:9f11e7a30865 15 DigitalIn sw[3] = {p23, p24, p25};
okini3939 0:9f11e7a30865 16 DigitalOut sw_pwr1(p21), sw_pwr2(p22);
okini3939 0:9f11e7a30865 17 SpiOLED oled(p5, p6, p7, p8, SpiOLED::LCD20x2);
okini3939 0:9f11e7a30865 18
okini3939 0:9f11e7a30865 19 volatile int received = 0;
okini3939 0:9f11e7a30865 20 struct AirData send_data;
okini3939 0:9f11e7a30865 21
okini3939 0:9f11e7a30865 22 struct Status stat, stat_gnd;
okini3939 0:9f11e7a30865 23 volatile int sw_fil = 0, sw_stat = 0, sw_cmd = 0, sw_xcmd = 0;
okini3939 0:9f11e7a30865 24 int mode = 0;
okini3939 0:9f11e7a30865 25 int rf_dual = 0;
okini3939 0:9f11e7a30865 26
okini3939 0:9f11e7a30865 27
okini3939 0:9f11e7a30865 28 void recvRf (struct GroundData *recv_data, int rssi) {
okini3939 0:9f11e7a30865 29
okini3939 0:9f11e7a30865 30 if (recv_data->type != DATA_TYPE_GROUND) return;
okini3939 0:9f11e7a30865 31
okini3939 0:9f11e7a30865 32 parseGps(recv_data->gps);
okini3939 0:9f11e7a30865 33 parseCompass(recv_data->compass);
okini3939 0:9f11e7a30865 34 stat.uptime = recv_data->uptime;
okini3939 0:9f11e7a30865 35 stat.battery = recv_data->battery;
okini3939 0:9f11e7a30865 36 stat.current = recv_data->current;
okini3939 0:9f11e7a30865 37 stat.amphour = recv_data->amphour;
okini3939 0:9f11e7a30865 38 stat.distance1 = recv_data->distance1;
okini3939 0:9f11e7a30865 39 stat.distance2 = recv_data->distance2;
okini3939 0:9f11e7a30865 40
okini3939 0:9f11e7a30865 41 received = recv_data->seq;
okini3939 0:9f11e7a30865 42 led2 = !led2;
okini3939 0:9f11e7a30865 43
okini3939 0:9f11e7a30865 44 log();
okini3939 0:9f11e7a30865 45 }
okini3939 0:9f11e7a30865 46
okini3939 0:9f11e7a30865 47 int getAnalog (int n) {
okini3939 0:9f11e7a30865 48 int ad;
okini3939 0:9f11e7a30865 49
okini3939 0:9f11e7a30865 50 __disable_irq();
okini3939 0:9f11e7a30865 51 ad = (adc[n].read() - 0.5) * 2.0 * (STICK_TILT + STICK_CHATTER);
okini3939 0:9f11e7a30865 52 __enable_irq();
okini3939 0:9f11e7a30865 53 if (ad < - STICK_CHATTER) {
okini3939 0:9f11e7a30865 54 ad += STICK_CHATTER;
okini3939 0:9f11e7a30865 55 } else
okini3939 0:9f11e7a30865 56 if (ad > STICK_CHATTER) {
okini3939 0:9f11e7a30865 57 ad -= STICK_CHATTER;
okini3939 0:9f11e7a30865 58 } else {
okini3939 0:9f11e7a30865 59 ad = 0;
okini3939 0:9f11e7a30865 60 }
okini3939 0:9f11e7a30865 61
okini3939 0:9f11e7a30865 62 if (ad < - STICK_TILT) ad = - STICK_TILT;
okini3939 0:9f11e7a30865 63 if (ad > STICK_TILT) ad = STICK_TILT;
okini3939 0:9f11e7a30865 64
okini3939 0:9f11e7a30865 65 return STICK_CENTER + ad;
okini3939 0:9f11e7a30865 66 }
okini3939 0:9f11e7a30865 67
okini3939 0:9f11e7a30865 68 int getSwitch () {
okini3939 0:9f11e7a30865 69 int i, r = 0;
okini3939 0:9f11e7a30865 70
okini3939 0:9f11e7a30865 71 sw_pwr1 = 0;
okini3939 0:9f11e7a30865 72 sw_pwr2 = 1;
okini3939 0:9f11e7a30865 73 for (volatile int w = 0; w < 10; w ++);
okini3939 0:9f11e7a30865 74 for (i = 0; i < 3; i ++) {
okini3939 0:9f11e7a30865 75 if (!sw[i]) r |= (1<<(i * 2));
okini3939 0:9f11e7a30865 76 }
okini3939 0:9f11e7a30865 77 sw_pwr1 = 1;
okini3939 0:9f11e7a30865 78 sw_pwr2 = 0;
okini3939 0:9f11e7a30865 79 for (volatile int w = 0; w < 10; w ++);
okini3939 0:9f11e7a30865 80 for (i = 0; i < 3; i ++) {
okini3939 0:9f11e7a30865 81 if (!sw[i]) r |= (2<<(i * 2));
okini3939 0:9f11e7a30865 82 }
okini3939 0:9f11e7a30865 83 return r;
okini3939 0:9f11e7a30865 84 }
okini3939 0:9f11e7a30865 85
okini3939 0:9f11e7a30865 86 void input () {
okini3939 0:9f11e7a30865 87 int a, b, c;
okini3939 0:9f11e7a30865 88
okini3939 0:9f11e7a30865 89 a = getSwitch();
okini3939 0:9f11e7a30865 90 if (a == sw_fil) {
okini3939 0:9f11e7a30865 91 b = sw_stat;
okini3939 0:9f11e7a30865 92 sw_stat = a;
okini3939 0:9f11e7a30865 93 c = (b ^ a) & ~a;
okini3939 0:9f11e7a30865 94 b = (b ^ a) & a;
okini3939 0:9f11e7a30865 95 if (b) sw_cmd = b;
okini3939 0:9f11e7a30865 96 if (c) sw_xcmd = c;
okini3939 0:9f11e7a30865 97 }
okini3939 0:9f11e7a30865 98 sw_fil = a;
okini3939 0:9f11e7a30865 99
okini3939 0:9f11e7a30865 100 if (sw_stat & 0x01) {
okini3939 0:9f11e7a30865 101 // start (ignition)
okini3939 0:9f11e7a30865 102 send_data.aileron = STICK_CENTER + STICK_TILT;
okini3939 0:9f11e7a30865 103 send_data.elevator = STICK_CENTER + STICK_TILT;
okini3939 0:9f11e7a30865 104 send_data.throttle = STICK_CENTER + STICK_TILT;
okini3939 0:9f11e7a30865 105 send_data.rudder = STICK_CENTER + STICK_TILT;
okini3939 0:9f11e7a30865 106 led5 = 1;
okini3939 0:9f11e7a30865 107 } else {
okini3939 0:9f11e7a30865 108 // normal
okini3939 0:9f11e7a30865 109 send_data.aileron = getAnalog(0);
okini3939 0:9f11e7a30865 110 send_data.elevator = getAnalog(1);
okini3939 0:9f11e7a30865 111 send_data.throttle = getAnalog(2);
okini3939 0:9f11e7a30865 112 send_data.rudder = getAnalog(3);
okini3939 0:9f11e7a30865 113
okini3939 0:9f11e7a30865 114 if (send_data.aileron == STICK_CENTER && send_data.elevator == STICK_CENTER && send_data.rudder == STICK_CENTER) {
okini3939 0:9f11e7a30865 115 led6 = 1;
okini3939 0:9f11e7a30865 116 } else {
okini3939 0:9f11e7a30865 117 led6 = 0;
okini3939 0:9f11e7a30865 118 }
okini3939 0:9f11e7a30865 119 if (send_data.throttle == STICK_CENTER) {
okini3939 0:9f11e7a30865 120 led7 = 1;
okini3939 0:9f11e7a30865 121 } else {
okini3939 0:9f11e7a30865 122 led7 = 0;
okini3939 0:9f11e7a30865 123 }
okini3939 0:9f11e7a30865 124 }
okini3939 0:9f11e7a30865 125
okini3939 0:9f11e7a30865 126 send_data.analog1 = getAnalog(4);
okini3939 0:9f11e7a30865 127 send_data.analog2 = getAnalog(5);
okini3939 0:9f11e7a30865 128 send_data.sw1 = STICK_CENTER - (sw_stat & 0x04 ? STICK_SWITCH : 0) + (sw_stat & 0x08 ? STICK_SWITCH : 0);
okini3939 0:9f11e7a30865 129 send_data.sw2 = STICK_CENTER - (sw_stat & 0x10 ? STICK_SWITCH : 0) + (sw_stat & 0x20 ? STICK_SWITCH : 0);
okini3939 0:9f11e7a30865 130 // printf(" A=%03x, E=%03x, T=%03x, R=%03x, X1=%03x, X2=%03x, sw=%02x\r\n", send_data.aileron, send_data.elevator, send_data.throttle, send_data.rudder, send_data.analog1, send_data.analog2, sw);
okini3939 0:9f11e7a30865 131 }
okini3939 0:9f11e7a30865 132
okini3939 0:9f11e7a30865 133 void display () {
okini3939 0:9f11e7a30865 134 if (sw_cmd & 0x02) {
okini3939 0:9f11e7a30865 135 mode ++;
okini3939 0:9f11e7a30865 136 sw_cmd = 0;
okini3939 0:9f11e7a30865 137 oled.cls();
okini3939 0:9f11e7a30865 138 }
okini3939 0:9f11e7a30865 139
okini3939 0:9f11e7a30865 140 switch (mode) {
okini3939 0:9f11e7a30865 141 case 0:
okini3939 0:9f11e7a30865 142 oled.locate(0, 0);
okini3939 0:9f11e7a30865 143 oled.printf("%4.1fV %4.1fA %4dmAh", (float)stat.battery / 1000.0, (float)stat.current / 1000.0, stat.amphour);
okini3939 0:9f11e7a30865 144 oled.locate(0, 1);
okini3939 0:9f11e7a30865 145 oled.printf("%4d %4d", stat.distance1, stat.distance2);
okini3939 0:9f11e7a30865 146 break;
okini3939 0:9f11e7a30865 147 case 1: // GPS
okini3939 0:9f11e7a30865 148 oled.locate(0, 0);
okini3939 0:9f11e7a30865 149 oled.printf("%8.5f %9.5f", (float)stat.gps_lat / 10000000.0, (float)stat.gps_lng / 10000000.0);
okini3939 0:9f11e7a30865 150 oled.locate(0, 1);
okini3939 0:9f11e7a30865 151 oled.printf("%4d %4d %4d %4d", stat.compass_x, stat.compass_y, stat.compass_z, stat.compass);
okini3939 0:9f11e7a30865 152 break;
okini3939 0:9f11e7a30865 153 case 2: // GPS
okini3939 0:9f11e7a30865 154 oled.locate(0, 0);
okini3939 0:9f11e7a30865 155 oled.printf("%4d %4d %4d %4d", stat.compass_x, stat.compass_y, stat.compass_z, stat.compass);
okini3939 0:9f11e7a30865 156 oled.locate(0, 1);
okini3939 0:9f11e7a30865 157 oled.printf("%4d %08d %06d", stat.gps_h, stat.gps_date, stat.gps_time);
okini3939 0:9f11e7a30865 158 break;
okini3939 0:9f11e7a30865 159 case 3: // GPS ground
okini3939 0:9f11e7a30865 160 oled.locate(0, 0);
okini3939 0:9f11e7a30865 161 oled.printf("%8.5f %9.5fG", (float)stat_gnd.gps_lat / 10000000.0, (float)stat_gnd.gps_lng / 10000000.0);
okini3939 0:9f11e7a30865 162 oled.locate(0, 1);
okini3939 0:9f11e7a30865 163 oled.printf("%4d %08d %06d", stat_gnd.gps_h, stat_gnd.gps_date, stat_gnd.gps_time);
okini3939 0:9f11e7a30865 164 break;
okini3939 0:9f11e7a30865 165 case 4: // stick
okini3939 0:9f11e7a30865 166 oled.locate(0, 0);
okini3939 0:9f11e7a30865 167 oled.printf("A%03x E%03x T%03x R%03x", send_data.aileron, send_data.elevator, send_data.throttle, send_data.rudder);
okini3939 0:9f11e7a30865 168 oled.locate(0, 1);
okini3939 0:9f11e7a30865 169 oled.printf(" %03x %03x %03x %03x", send_data.analog1, send_data.analog2, send_data.sw1, send_data.sw2);
okini3939 0:9f11e7a30865 170 break;
okini3939 0:9f11e7a30865 171 default:
okini3939 0:9f11e7a30865 172 mode = 0;
okini3939 0:9f11e7a30865 173 break;
okini3939 0:9f11e7a30865 174 }
okini3939 0:9f11e7a30865 175
okini3939 0:9f11e7a30865 176 oled.locate(19, 0);
okini3939 0:9f11e7a30865 177 oled.putc((received & 1) ? (rf_dual == 3 ? '*' : '.') : ' ');
okini3939 0:9f11e7a30865 178 /*
okini3939 0:9f11e7a30865 179 pc.printf("Drone sec=%3d %4.1fV %4.1fA %4dmAh", stat.uptime, (float)stat.battery / 1000.0, (float)stat.current / 1000.0, stat.amphour / 1000);
okini3939 0:9f11e7a30865 180 pc.printf(" dis1=%4d dis2=%4d\r\n", stat.distance1, stat.distance2);
okini3939 0:9f11e7a30865 181 pc.printf(" lat=%8.5f lng=%9.5f", (float)stat.gps_lat / 10000000.0, (float)stat.gps_lng / 10000000.0);
okini3939 0:9f11e7a30865 182 pc.printf(" h=%d %08d %06d\r\n", stat.gps_h, stat.gps_date, stat.gps_time);
okini3939 0:9f11e7a30865 183 pc.printf(" x=%4d y=4d z=%4d deg=%4d", stat.compass_x, stat.compass_y, stat.compass_z, stat.compass);
okini3939 0:9f11e7a30865 184 pc.printf(" sat=%d type=%d flg=%d\r\n", stat.gps_sat, stat.gps_type, stat.gps_flg);
okini3939 0:9f11e7a30865 185 */
okini3939 0:9f11e7a30865 186 }
okini3939 0:9f11e7a30865 187
okini3939 0:9f11e7a30865 188 void center () {
okini3939 0:9f11e7a30865 189 int flg = 1;
okini3939 0:9f11e7a30865 190
okini3939 0:9f11e7a30865 191 for (;;) {
okini3939 0:9f11e7a30865 192 input();
okini3939 0:9f11e7a30865 193
okini3939 0:9f11e7a30865 194 if (send_data.aileron == STICK_CENTER && send_data.elevator == STICK_CENTER && send_data.rudder == STICK_CENTER &&
okini3939 0:9f11e7a30865 195 send_data.throttle == STICK_CENTER && send_data.analog1 == STICK_CENTER && send_data.analog2 == STICK_CENTER) {
okini3939 0:9f11e7a30865 196 break;
okini3939 0:9f11e7a30865 197 }
okini3939 0:9f11e7a30865 198
okini3939 0:9f11e7a30865 199 if (flg) {
okini3939 0:9f11e7a30865 200 flg = 0;
okini3939 0:9f11e7a30865 201 oled.cls();
okini3939 0:9f11e7a30865 202 oled.locate(6, 0);
okini3939 0:9f11e7a30865 203 oled.printf("STICKS ARE");
okini3939 0:9f11e7a30865 204 oled.locate(6, 1);
okini3939 0:9f11e7a30865 205 oled.printf("NOT IN CENTER!");
okini3939 0:9f11e7a30865 206 }
okini3939 0:9f11e7a30865 207
okini3939 0:9f11e7a30865 208 oled.locate(0, 0);
okini3939 0:9f11e7a30865 209 oled.printf("%4d", send_data.analog2 - STICK_CENTER);
okini3939 0:9f11e7a30865 210 oled.locate(0, 1);
okini3939 0:9f11e7a30865 211 oled.printf("%4d", send_data.analog1 - STICK_CENTER);
okini3939 0:9f11e7a30865 212 led5 = !led5;
okini3939 0:9f11e7a30865 213 wait_ms(200);
okini3939 0:9f11e7a30865 214 }
okini3939 0:9f11e7a30865 215 oled.cls();
okini3939 0:9f11e7a30865 216 }
okini3939 0:9f11e7a30865 217
okini3939 0:9f11e7a30865 218 void fault () {
okini3939 0:9f11e7a30865 219 for (;;) {
okini3939 0:9f11e7a30865 220 led5 = !led5;
okini3939 0:9f11e7a30865 221 wait_ms(200);
okini3939 0:9f11e7a30865 222 }
okini3939 0:9f11e7a30865 223 }
okini3939 0:9f11e7a30865 224
okini3939 0:9f11e7a30865 225 void adjust () {
okini3939 0:9f11e7a30865 226 int i;
okini3939 0:9f11e7a30865 227
okini3939 0:9f11e7a30865 228 if (stat_gnd.gps_date && stat_gnd.gps_time) {
okini3939 0:9f11e7a30865 229 struct tm t;
okini3939 0:9f11e7a30865 230 t.tm_year = ((stat_gnd.gps_date / 10000) % 10000) - 1900;
okini3939 0:9f11e7a30865 231 t.tm_mon = ((stat_gnd.gps_date / 100) % 100) - 1;
okini3939 0:9f11e7a30865 232 t.tm_mday = (stat_gnd.gps_date % 100);
okini3939 0:9f11e7a30865 233 t.tm_hour = (stat_gnd.gps_time / 10000) % 100;
okini3939 0:9f11e7a30865 234 t.tm_min = (stat_gnd.gps_time / 100) % 100;
okini3939 0:9f11e7a30865 235 t.tm_sec = stat_gnd.gps_time % 100;
okini3939 0:9f11e7a30865 236 time_t gpstime = mktime(&t) + (9 * 60 * 60);
okini3939 0:9f11e7a30865 237
okini3939 0:9f11e7a30865 238 time_t s = time(NULL);
okini3939 0:9f11e7a30865 239 i = s > gpstime ? s - gpstime : gpstime - s;
okini3939 0:9f11e7a30865 240 if (i > 1 && t.tm_sec >= 15 && t.tm_sec < 45 && t.tm_year >= 115 && t.tm_year < 138) {
okini3939 0:9f11e7a30865 241 set_time(gpstime);
okini3939 0:9f11e7a30865 242 printf("time adjust %d -> %d\r\n", s, gpstime);
okini3939 0:9f11e7a30865 243 }
okini3939 0:9f11e7a30865 244 }
okini3939 0:9f11e7a30865 245 }
okini3939 0:9f11e7a30865 246
okini3939 0:9f11e7a30865 247 int main() {
okini3939 0:9f11e7a30865 248 int count = 0;
okini3939 0:9f11e7a30865 249 Timer t;
okini3939 0:9f11e7a30865 250
okini3939 0:9f11e7a30865 251 PHY_PowerDown();
okini3939 0:9f11e7a30865 252 // Peripheral_PowerDown(LPC1768_PCONP_PCUSB|LPC1768_PCONP_PCENET|LPC1768_PCONP_PCI2S);
okini3939 0:9f11e7a30865 253 Peripheral_PowerDown(LPC1768_PCONP_PCENET|LPC1768_PCONP_PCI2S);
okini3939 0:9f11e7a30865 254 sw[0].mode(PullUp);
okini3939 0:9f11e7a30865 255 sw[1].mode(PullUp);
okini3939 0:9f11e7a30865 256 sw[2].mode(PullUp);
okini3939 0:9f11e7a30865 257 memset(&stat, 0, sizeof(stat));
okini3939 0:9f11e7a30865 258 pc.baud(115200);
okini3939 0:9f11e7a30865 259 pc.printf("--- Drone Ground ---\r\n");
okini3939 0:9f11e7a30865 260
okini3939 0:9f11e7a30865 261 center();
okini3939 0:9f11e7a30865 262 oled.printf("Suga koubou - Drone");
okini3939 0:9f11e7a30865 263
okini3939 0:9f11e7a30865 264 if (initRf()) {
okini3939 0:9f11e7a30865 265 oled.cls();
okini3939 0:9f11e7a30865 266 oled.printf("*** RF error!");
okini3939 0:9f11e7a30865 267 fault();
okini3939 0:9f11e7a30865 268 }
okini3939 0:9f11e7a30865 269 initGps();
okini3939 0:9f11e7a30865 270
okini3939 0:9f11e7a30865 271 wait_ms(500);
okini3939 0:9f11e7a30865 272 adjust();
okini3939 0:9f11e7a30865 273 initMsd();
okini3939 0:9f11e7a30865 274 led1 = 1;
okini3939 0:9f11e7a30865 275
okini3939 0:9f11e7a30865 276 t.start();
okini3939 0:9f11e7a30865 277 for (;;) {
okini3939 0:9f11e7a30865 278 pollRf();
okini3939 0:9f11e7a30865 279
okini3939 0:9f11e7a30865 280 if (t.read_ms() >= (rf_dual == 3 ? 100 : 200)) {
okini3939 0:9f11e7a30865 281 t.reset();
okini3939 0:9f11e7a30865 282
okini3939 0:9f11e7a30865 283 // stick
okini3939 0:9f11e7a30865 284 input();
okini3939 0:9f11e7a30865 285 // send
okini3939 0:9f11e7a30865 286 if (sendRf(&send_data)) {
okini3939 0:9f11e7a30865 287 led4 = 1;
okini3939 0:9f11e7a30865 288 led5 = 1;
okini3939 0:9f11e7a30865 289 } else {
okini3939 0:9f11e7a30865 290 led4 = 0;
okini3939 0:9f11e7a30865 291 }
okini3939 0:9f11e7a30865 292 led1 = !led1;
okini3939 0:9f11e7a30865 293
okini3939 0:9f11e7a30865 294 if (received) {
okini3939 0:9f11e7a30865 295 // oled
okini3939 0:9f11e7a30865 296 display();
okini3939 0:9f11e7a30865 297 led5 = 0;
okini3939 0:9f11e7a30865 298 received = 0;
okini3939 0:9f11e7a30865 299 count = 0;
okini3939 0:9f11e7a30865 300 } else {
okini3939 0:9f11e7a30865 301 count ++;
okini3939 0:9f11e7a30865 302 if (count > 20) {
okini3939 0:9f11e7a30865 303 // lost
okini3939 0:9f11e7a30865 304 led5 = 1;
okini3939 0:9f11e7a30865 305 }
okini3939 0:9f11e7a30865 306 if (count % 2 == 0) {
okini3939 0:9f11e7a30865 307 display();
okini3939 0:9f11e7a30865 308 }
okini3939 0:9f11e7a30865 309 if (count % 10 == 0) {
okini3939 0:9f11e7a30865 310 log();
okini3939 0:9f11e7a30865 311 }
okini3939 0:9f11e7a30865 312 }
okini3939 0:9f11e7a30865 313
okini3939 0:9f11e7a30865 314 // clock
okini3939 0:9f11e7a30865 315 adjust();
okini3939 0:9f11e7a30865 316 }
okini3939 0:9f11e7a30865 317
okini3939 0:9f11e7a30865 318 }
okini3939 0:9f11e7a30865 319 }