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

main.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 "EthernetPowerControl.h"
okini3939 0:4a37291f07ca 4
okini3939 0:4a37291f07ca 5 DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
okini3939 0:4a37291f07ca 6 Serial pc(USBTX, USBRX);
okini3939 0:4a37291f07ca 7 AnalogIn ad_battery(p19), ad_current(p20);
okini3939 0:4a37291f07ca 8
okini3939 0:4a37291f07ca 9 volatile int received = 0, timeout = 0;
okini3939 0:4a37291f07ca 10 struct GroundData send_data;
okini3939 0:4a37291f07ca 11 struct Status stat;
okini3939 0:4a37291f07ca 12 int rf_dual = 0;
okini3939 0:4a37291f07ca 13
okini3939 0:4a37291f07ca 14 // radio control
okini3939 0:4a37291f07ca 15 void recvRf (struct AirData *recv_data, int rssi) {
okini3939 0:4a37291f07ca 16
okini3939 0:4a37291f07ca 17 if (recv_data->type != DATA_TYPE_AIR) return;
okini3939 0:4a37291f07ca 18
okini3939 0:4a37291f07ca 19 // send S.Bus
okini3939 0:4a37291f07ca 20 setSBus(1, recv_data->aileron);
okini3939 0:4a37291f07ca 21 setSBus(2, recv_data->elevator);
okini3939 0:4a37291f07ca 22 setSBus(3, recv_data->throttle);
okini3939 0:4a37291f07ca 23 setSBus(4, recv_data->rudder);
okini3939 0:4a37291f07ca 24 setSBus(5, recv_data->analog1);
okini3939 0:4a37291f07ca 25 setSBus(6, recv_data->analog2);
okini3939 0:4a37291f07ca 26 setSBus(7, recv_data->sw1);
okini3939 0:4a37291f07ca 27 // printf("A=%04d E=%04d T=%04d R=%04d M=%04d G=%04d\r\n",
okini3939 0:4a37291f07ca 28 // recv_data->aileron, recv_data->elevator, recv_data->throttle, recv_data->rudder, recv_data->mode, recv_data->gain);
okini3939 0:4a37291f07ca 29 setFailsafeSBus(recv_data->sw2 <= 0x400 ? 0 : 1);
okini3939 0:4a37291f07ca 30
okini3939 0:4a37291f07ca 31 if (rf_dual != (recv_data->flags & 0x03)) {
okini3939 0:4a37291f07ca 32 rf_dual = recv_data->flags & 0x03;
okini3939 0:4a37291f07ca 33 printf("rf_dual %d\r\n", rf_dual);
okini3939 0:4a37291f07ca 34 }
okini3939 0:4a37291f07ca 35 received = recv_data->seq;
okini3939 0:4a37291f07ca 36 timeout = 8; // lost time (*250ms)
okini3939 0:4a37291f07ca 37 led2 = !led2;
okini3939 0:4a37291f07ca 38 }
okini3939 0:4a37291f07ca 39
okini3939 0:4a37291f07ca 40 // lost control
okini3939 0:4a37291f07ca 41 void lost () {
okini3939 0:4a37291f07ca 42 int i;
okini3939 0:4a37291f07ca 43 for (i = 1; i <= 7; i ++) {
okini3939 0:4a37291f07ca 44 setSBus(i, 0x400);
okini3939 0:4a37291f07ca 45 }
okini3939 0:4a37291f07ca 46 led2 = 0;
okini3939 0:4a37291f07ca 47 printf("lost\r\n");
okini3939 0:4a37291f07ca 48 }
okini3939 0:4a37291f07ca 49
okini3939 0:4a37291f07ca 50 // telemetry
okini3939 0:4a37291f07ca 51 void send () {
okini3939 0:4a37291f07ca 52
okini3939 0:4a37291f07ca 53 send_data.type = 0;
okini3939 0:4a37291f07ca 54 send_data.uptime = stat.uptime;
okini3939 0:4a37291f07ca 55 send_data.current = stat.current;
okini3939 0:4a37291f07ca 56 send_data.battery = stat.battery;
okini3939 0:4a37291f07ca 57 send_data.amphour = stat.amphour;
okini3939 0:4a37291f07ca 58 send_data.distance1 = stat.distance1;
okini3939 0:4a37291f07ca 59 send_data.distance2 = stat.distance2;
okini3939 0:4a37291f07ca 60
okini3939 0:4a37291f07ca 61 if (sendRf(&send_data)) {
okini3939 0:4a37291f07ca 62 led4 = 1;
okini3939 0:4a37291f07ca 63 }
okini3939 0:4a37291f07ca 64 }
okini3939 0:4a37291f07ca 65
okini3939 0:4a37291f07ca 66 int main() {
okini3939 0:4a37291f07ca 67 int i, count = 0, timeout = 0;
okini3939 0:4a37291f07ca 68 Timer t, uptime;
okini3939 0:4a37291f07ca 69
okini3939 0:4a37291f07ca 70 PHY_PowerDown();
okini3939 0:4a37291f07ca 71 Peripheral_PowerDown(LPC1768_PCONP_PCUSB|LPC1768_PCONP_PCENET|LPC1768_PCONP_PCI2S);
okini3939 0:4a37291f07ca 72
okini3939 0:4a37291f07ca 73 LPC_SC->CCLKCFG = 5; // 48MHz
okini3939 0:4a37291f07ca 74 SystemCoreClockUpdate();
okini3939 0:4a37291f07ca 75 LPC_TIM3->PR = (SystemCoreClock / 4 / 1000000) - 1; // us_ticker
okini3939 0:4a37291f07ca 76
okini3939 0:4a37291f07ca 77 memset(&stat, 0, sizeof(stat));
okini3939 0:4a37291f07ca 78 pc.baud(115200);
okini3939 0:4a37291f07ca 79 pc.printf("--- Drone Air ---\r\n");
okini3939 0:4a37291f07ca 80
okini3939 0:4a37291f07ca 81 initSBus();
okini3939 0:4a37291f07ca 82 initCan();
okini3939 0:4a37291f07ca 83 wait_ms(200);
okini3939 0:4a37291f07ca 84 if (initRf()) {
okini3939 0:4a37291f07ca 85 // return -1;
okini3939 0:4a37291f07ca 86 rf_dual = 3;
okini3939 0:4a37291f07ca 87 }
okini3939 0:4a37291f07ca 88
okini3939 0:4a37291f07ca 89 uptime.start();
okini3939 0:4a37291f07ca 90 t.start();
okini3939 0:4a37291f07ca 91 led1 = 1;
okini3939 0:4a37291f07ca 92 for (;;) {
okini3939 0:4a37291f07ca 93 pollRf();
okini3939 0:4a37291f07ca 94
okini3939 0:4a37291f07ca 95 if (t.read_ms() >= 250) { // 4Hz
okini3939 0:4a37291f07ca 96 t.reset();
okini3939 0:4a37291f07ca 97
okini3939 0:4a37291f07ca 98 // adc
okini3939 0:4a37291f07ca 99 __disable_irq();
okini3939 0:4a37291f07ca 100 stat.battery = (ad_battery * 3300 * 72) / 10; // 62k:10k (mV)
okini3939 0:4a37291f07ca 101 i = (ad_current * 3300 * 40) / 30; // 10k:30k (mV)
okini3939 0:4a37291f07ca 102 __enable_irq();
okini3939 0:4a37291f07ca 103 stat.current = (i * 50) / 4; // 4V/50A (mA)
okini3939 0:4a37291f07ca 104 stat.current_sum += stat.current;
okini3939 0:4a37291f07ca 105 stat.current_count += 1;
okini3939 0:4a37291f07ca 106 stat.amphour = stat.current_sum / (3600 * 4); // 0.25s (mAh)
okini3939 0:4a37291f07ca 107
okini3939 0:4a37291f07ca 108 count ++;
okini3939 0:4a37291f07ca 109
okini3939 0:4a37291f07ca 110 if (timeout) {
okini3939 0:4a37291f07ca 111 timeout --;
okini3939 0:4a37291f07ca 112 if (timeout == 0) {
okini3939 0:4a37291f07ca 113 // lost
okini3939 0:4a37291f07ca 114 lost();
okini3939 0:4a37291f07ca 115 }
okini3939 0:4a37291f07ca 116 }
okini3939 0:4a37291f07ca 117 }
okini3939 0:4a37291f07ca 118
okini3939 0:4a37291f07ca 119 if ((count >= 2 && (rf_dual == 3 || received)) || count >= 40) {
okini3939 0:4a37291f07ca 120 // send telemetry
okini3939 0:4a37291f07ca 121 stat.uptime = uptime.read();
okini3939 0:4a37291f07ca 122 send();
okini3939 0:4a37291f07ca 123 count = 0;
okini3939 0:4a37291f07ca 124 received = 0;
okini3939 0:4a37291f07ca 125 led1 = !led1;
okini3939 0:4a37291f07ca 126 led4 = 0;
okini3939 0:4a37291f07ca 127 }
okini3939 0:4a37291f07ca 128 }
okini3939 0:4a37291f07ca 129 }