DJI NAZA-M controller (multi copter side) see: https://developer.mbed.org/users/okini3939/notebook/drone/
Dependencies: FutabaSBUS NECnfc mbed
main.cpp
- Committer:
- okini3939
- Date:
- 2016-05-19
- Revision:
- 1:32cd1cf5d5b1
- Parent:
- 0:4a37291f07ca
File content as of revision 1:32cd1cf5d5b1:
#include "mbed.h" #include "drone.h" #include "EthernetPowerControl.h" DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4); Serial pc(USBTX, USBRX); AnalogIn ad_battery(p19), ad_current(p20); volatile int received = 0, timeout = 0; struct GroundData send_data; struct Status stat; int rf_dual = 0; // radio control void recvRf (struct AirData *recv_data, int rssi) { if (recv_data->type != DATA_TYPE_AIR) return; // send S.Bus setSBus(1, recv_data->aileron); setSBus(2, recv_data->elevator); setSBus(3, recv_data->throttle); setSBus(4, recv_data->rudder); setSBus(5, recv_data->analog1); setSBus(6, recv_data->analog2); setSBus(7, recv_data->sw1); // printf("A=%04d E=%04d T=%04d R=%04d M=%04d G=%04d\r\n", // recv_data->aileron, recv_data->elevator, recv_data->throttle, recv_data->rudder, recv_data->mode, recv_data->gain); setFailsafeSBus(recv_data->sw2 <= 0x400 ? 0 : 1); if (rf_dual != (recv_data->flags & 0x03)) { rf_dual = recv_data->flags & 0x03; printf("rf_dual %d\r\n", rf_dual); } received = recv_data->seq; timeout = 8; // lost time (*250ms) led2 = !led2; } // lost control void lost () { int i; for (i = 1; i <= 7; i ++) { setSBus(i, 0x400); } led2 = 0; printf("lost\r\n"); } // telemetry void send () { send_data.type = 0; send_data.uptime = stat.uptime; send_data.current = stat.current; send_data.battery = stat.battery; send_data.amphour = stat.amphour; send_data.distance1 = stat.distance1; send_data.distance2 = stat.distance2; if (sendRf(&send_data)) { led4 = 1; } } int main() { int i, count = 0, timeout = 0; Timer t, uptime; PHY_PowerDown(); Peripheral_PowerDown(LPC1768_PCONP_PCUSB|LPC1768_PCONP_PCENET|LPC1768_PCONP_PCI2S); LPC_SC->CCLKCFG = 5; // 48MHz SystemCoreClockUpdate(); LPC_TIM3->PR = (SystemCoreClock / 4 / 1000000) - 1; // us_ticker memset(&stat, 0, sizeof(stat)); pc.baud(115200); pc.printf("--- Drone Air ---\r\n"); initSBus(); initCan(); wait_ms(200); if (initRf()) { // return -1; rf_dual = 3; } uptime.start(); t.start(); led1 = 1; for (;;) { pollRf(); if (t.read_ms() >= 250) { // 4Hz t.reset(); // adc __disable_irq(); stat.battery = (ad_battery * 3300 * 72) / 10; // 62k:10k (mV) i = (ad_current * 3300 * 40) / 30; // 10k:30k (mV) __enable_irq(); stat.current = (i * 50) / 4; // 4V/50A (mA) stat.current_sum += stat.current; stat.current_count += 1; stat.amphour = stat.current_sum / (3600 * 4); // 0.25s (mAh) count ++; if (timeout) { timeout --; if (timeout == 0) { // lost lost(); } } } if ((count >= 2 && (rf_dual == 3 || received)) || count >= 40) { // send telemetry stat.uptime = uptime.read(); send(); count = 0; received = 0; led1 = !led1; led4 = 0; } } }