From 4892dcf8809161c715cf6b80d669b89647331c82 Mon Sep 17 00:00:00 2001 From: Kingkone Date: Mon, 17 Dec 2018 21:13:24 +0100 Subject: [PATCH] EEPROM config, cli --- .DS_Store | Bin 6148 -> 6148 bytes config.inav | 9 +- src/lib/CLI.h | 128 ++++++++++++++ src/lib/MSP.h | 5 +- src/main.cpp | 450 +++++++++++++++++++++++++++++++++----------------- src/main.h | 55 ++++++ 6 files changed, 494 insertions(+), 153 deletions(-) create mode 100755 src/lib/CLI.h create mode 100644 src/main.h diff --git a/.DS_Store b/.DS_Store index 651941c7305c1c9c108e0b9f94521f7a0d950d08..f648c5c8485f385a7fcece7e3f3a44e36930686c 100644 GIT binary patch delta 148 zcmZoMXffDO&pMf(Eu)^Dp@boop%_SJGUPF&=cF43C+FuDFdzV$irjn`mz2_yWCn&C crm1OQWf&?_2&gE?fZ4J!c_HIwc8V!Z delta 148 zcmZoMXffDO&sxvHP{feRkORbt48=gQI49jOI5|JJfB^w8)a2&7xTKVpBr`DFFilMZ bE5xEEg`k>(jLCVdl^athFm7h&_{$Ff)juOv diff --git a/config.inav b/config.inav index 5d12814..cd08e22 100644 --- a/config.inav +++ b/config.inav @@ -1,3 +1,4 @@ +# mixer mmix 0 1.000 -1.000 1.000 -1.000 mmix 1 1.000 -1.000 -1.000 1.000 mmix 2 1.000 1.000 1.000 1.000 @@ -45,7 +46,7 @@ osd_layout 0 9 1 2 H osd_layout 0 11 1 3 H osd_layout 0 12 1 4 H osd_layout 0 14 25 10 V -osd_layout 0 15 1 0 H +osd_layout 0 15 25 9 V osd_layout 0 20 15 0 V osd_layout 0 21 4 0 V osd_layout 0 28 23 11 H @@ -61,7 +62,10 @@ set accgain_x = 4085 set accgain_y = 4085 set accgain_z = 4104 set mag_hardware = NONE -set baro_hardware = NONE +set magzero_x = 93 +set magzero_y = -88 +set magzero_z = -19 +set baro_hardware = BMP280 set pitot_hardware = NONE set serialrx_provider = IBUS set min_throttle = 1065 @@ -77,5 +81,4 @@ profile 1 # battery_profile battery_profile 1 - save diff --git a/src/lib/CLI.h b/src/lib/CLI.h new file mode 100755 index 0000000..3f07813 --- /dev/null +++ b/src/lib/CLI.h @@ -0,0 +1,128 @@ +// ------------------------------------------------------------------------------------------------------ +// +// # DESCRIPTION : Basic CLI (Command Line Interpreter) library for Arduino. +// +// The CLI class first allows the user to register commands (i.e. a combination of a name and a function +// address) into a linked list. +// Then, on every LF (Line Feed) char reception on the Serial Interface, the class checks whether the +// input command name matches a registered one. In this case, the corresponding function is executed. +// +// NB1 : Each command name is 8 characters MAX by default. +// NB2 : The functions associated to the commands shall be "void function(void)" typed. +// +// # USAGE : +// 1. in the setup() Arduino sketch function, register all your commands using RegisterCmd() method +// and open the serial port. +// 2. in the loop() Arduino sketch function, call the Run() method to allow CLI execution. +// +// NB : If you don't use the Arduino serial monitor, make sure your terminal application is configured +// with a 'LF' char for a New Line ('CR' char is ignored). +// ------------------------------------------------------------------------------------------------------ + +#ifndef CLI_H +#define CLI_H + +#include "HardwareSerial.h" + +#define CMD_NAME_MAX_LENGTH 8 + +typedef void (*t_fct)(void); // typedef for function pointer + +// typedef for the linked list of command lines +typedef struct t_cmd { + char name[CMD_NAME_MAX_LENGTH + 1]; + t_fct fct; + struct t_cmd *next; +}; + + +class Cli{ + HardwareSerial& _serial; // serial interface to be used + t_cmd *_cmdList; // linked list of commands + + public : + // Constructor + Cli(HardwareSerial& serial) : _serial(serial) { _cmdList = NULL;} + + // Destructor + ~Cli() + { + t_cmd *current, *next; + for (current = _cmdList; current; current = next) + { + next = current->next; + delete(current); + } + } + + // RegisterCmd() function + // Allow the user to register a new user command + // Return an error (-1) when : + // - the passed command name is null or too long + // - the passed function @ is null + // - no new memory can be allocated + // else return 0 + char RegisterCmd(const char name[], t_fct fct) + { + unsigned int length; + // check params + if (fct == NULL) return -1; + for (length = 0; name[length] ; length++); + if ((!length) || (length > CMD_NAME_MAX_LENGTH)) return -1; + + // create new command + t_cmd *newCmd = new(t_cmd); + if (newCmd == NULL) return -1; // allocation failure + newCmd->fct = fct; + for (unsigned int i=0; i<=length; i++) newCmd->name[i] = name[i]; + + // attach the new command to the linked list + if (_cmdList == NULL) + { + _cmdList = newCmd ; _cmdList->next = NULL; + } + else + { + newCmd->next = _cmdList; + _cmdList = newCmd; + } + return 0; + } + + // Run() function + // To be called in loop() Arduino sketch function + // Read the serial interface, and execute a given function when the input text matches + void Run(void) + { + static char inData[CMD_NAME_MAX_LENGTH + 1]; + char newChar; + static unsigned int index = 0; + t_cmd *current; + unsigned char i; + + while (_serial.available() > 0) + { + newChar = _serial.read(); + if (newChar == '\r') continue; // ignore CR chars + if (newChar == '\n') + { + inData[index] = NULL; + index = 0; + for (current = _cmdList; current != NULL ; current = current->next) + { + for(i=0; (inData[i] == current->name[i]) && current->name[i]; i++); + if ((!inData[i]) && (!current->name[i])) + { + current->fct(); + break; // execute one command only + } + } + } + else if (index == CMD_NAME_MAX_LENGTH) index = 0; // restart acquisition from zero + else inData[index++] = newChar; + } + } +}; + + +#endif // CLI_H diff --git a/src/lib/MSP.h b/src/lib/MSP.h index 578b848..61a2214 100644 --- a/src/lib/MSP.h +++ b/src/lib/MSP.h @@ -69,7 +69,8 @@ #define MSP_SET_RAW_RC 200 // 8 rc chan #define MSP_SET_RAW_GPS 201 // fix, numsat, lat, lon, alt, speed #define MSP_SET_WP 209 // sets a given WP (WP#, lat, lon, alt, flags) - +// v2 commands +#define MSP2_ESP32 0x2040 // bits of getActiveModes() return value #define MSP_MODE_ARM 0 @@ -665,7 +666,7 @@ class MSP { void send(uint8_t messageID, void * payload, uint8_t size); - void sendV2(uint16_t messageID, void * payload, uint16_t size); + void sendv2(uint16_t messageID, void * payload, uint16_t size); bool recv(uint8_t * messageID, void * payload, uint8_t maxSize, uint8_t * recvSize); diff --git a/src/main.cpp b/src/main.cpp index 87d201f..125a4bf 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,31 +1,26 @@ +#define RADARESP32 +//#define RADARATMEGA168 + #include #include -#include #include -#include +#include +#include -const uint8_t activeSymbol[] PROGMEM = { - B00000000, - B00000000, - B00011000, - B00100100, - B01000010, - B01000010, - B00100100, - B00011000 -}; +#ifdef RADARESP32 +#include +#include +#include -const uint8_t inactiveSymbol[] PROGMEM = { - B00000000, - B00000000, - B00000000, - B00000000, - B00011000, - B00011000, - B00000000, - B00000000 -}; +#define rxPin 17 +#define txPin 23 +#endif +#ifdef RADARATMEGA168 +#define rxPin 17 +#define txPin 23 +#endif +// #define SCK 5 // GPIO5 - SX1278's SCK #define MISO 19 // GPIO19 - SX1278's MISO #define MOSI 27 // GPIO27 - SX1278's MOSI @@ -34,58 +29,138 @@ const uint8_t inactiveSymbol[] PROGMEM = { #define DI0 26 // GPIO26 - SX1278's IRQ (interrupt request) #define BAND 868E6 // 915E6 +#define CFGVER 1 +// ----------------------------------------------------------------------------- global vars +config cfg; SSD1306 display (0x3c, 4, 15); - -#define rxPin 17 -#define txPin 23 - MSP msp; +bool booted = 0; -int sendInterval = 500; long sendLastTime = 0; -int displayInterval = 100; long displayLastTime = 0; +long pdLastTime = 0; -struct planeData { - char header[7]; - byte loraAddress; - char planeName[20]; - bool armState; - msp_raw_gps_t gps; -}; -struct planesData { - uint8_t waypointNumber; - long lastUpdate; - planeData pd; -}; -planeData pd; -planeData pdIn; -planesData pds[5]; -planeData fakepd; -char planeFC[20]; +planeData pd; // our uav data +planeData pdIn; // new air packet +planesData pds[5]; // uav db +planeData fakepd; // debugging +char planeFC[20]; // uav fc name -bool loraRX = 0; // new packet flag -bool loraRXd = 0; // new packet display flag -bool loraTX = 0; -planeData loraMsg; -byte loraAddress = 0x01; // our uniq loraAddress +bool loraRX = 0; // display RX +bool loraTX = 0; // display TX +planeData loraMsg; // incoming packet -// ----------------------------------------------------------------------------- String seperator split -String getValue(String data, char separator, int index) { - int found = 0; - int strIndex[] = {0, -1}; - int maxIndex = data.length()-1; - for(int i=0; i<=maxIndex && found<=index; i++){ - if(data.charAt(i)==separator || i==maxIndex){ - found++; - strIndex[0] = strIndex[1]+1; - strIndex[1] = (i == maxIndex) ? i+1 : i; +#ifdef RADARESP32 +// ----------------------------------------------------------------------------- EEPROM / config +void saveConfig () { + //size_t size = sizeof(cfg); + //EEPROM.begin(size * 2); + for(size_t i = 0; i < sizeof(cfg); i++) { + char data = ((char *)&cfg)[i]; + EEPROM.write(i, data); + } + EEPROM.commit(); +} +void initConfig () { + size_t size = sizeof(cfg); + EEPROM.begin(size * 2); + for(size_t i = 0; i < size; i++) { + char data = EEPROM.read(i); + ((char *)&cfg)[i] = data; + } + if (cfg.configVersion != CFGVER) { // write default config + cfg.configVersion = CFGVER; + String("ADS-RC").toCharArray(cfg.loraHeader,7); // protocol identifier + cfg.loraAddress = 1; // local lora address + cfg.loraFrequency = 868E6; // 433E6, 868E6, 915E6 + cfg.loraBandwidth = 250000;// 250000 bps + cfg.loraCodingRate4 = 6; // 6? + cfg.loraSpreadingFactor = 7; // 7? + cfg.intervalSend = 300; // in ms + random + cfg.intervalDisplay = 100; // in ms + cfg.intervalStatus = 1000; // in ms + cfg.uavTimeout = 10; // in sec + cfg.mspTX = 23; // pin for msp serial TX + cfg.mspRX = 17; // pin for msp serial RX + cfg.mspPOI = 1; // POI type: 1 (Wayponit), 2 (Plane) + cfg.debugOutput = false; + cfg.debugFakeWPs = false; + cfg.debugFakePlanes = false; + cfg.debugFakeMoving = false; + EEPROM.begin(size * 2); + for(size_t i = 0; i < size; i++) { + char data = ((char *)&cfg)[i]; + EEPROM.write(i, data); } + EEPROM.commit(); + Serial.println("Default config written to EEPROM!"); + } else { + Serial.println("Config found!"); } +} +// ----------------------------------------------------------------------------- CLI +Cli cli = Cli(Serial); +void cliLog (String log) { + if (cfg.debugOutput) { + if (booted) { + Serial.print("LOG: "); + Serial.println(log); + } else { + Serial.println(log); + } + } +} +void cliStatus(void) { + Serial.println(); + Serial.println("================== Status =================="); + Serial.print("FC: "); + Serial.println(planeFC); + Serial.print("Name: "); + Serial.println(pd.planeName); + Serial.print("Arm state: "); + Serial.println(pd.armState ? "ARMED" : "DISARMED"); + Serial.print("GPS: "); + Serial.println(pd.gps.fixType ? String(pd.gps.numSat) + " Sats" : "no fix"); + +} +void cliHelp(void) { + Serial.println(); + Serial.println("================= Commands ================="); + Serial.println("status - Show whats going on"); + Serial.println("help - List all commands"); + Serial.println("config - List all settings"); + Serial.println("reboot - Reset MCU and radio"); + //Serial.println("gpspos - Show last GPS position"); + //Serial.println("fcpass - start FC passthru mode"); + Serial.println("debug - Toggle debug mode"); +} +void cliConfig(void) { + Serial.println(); + Serial.println("=============== Configuration =============="); +} +void cliDebug(void) { + Serial.println(); + cfg.debugOutput = !cfg.debugOutput; + saveConfig(); + Serial.println("CLI debugging: " + String(cfg.debugOutput)); +} +void cliReboot(void) { + Serial.println(); + Serial.println("Rebooting ..."); + ESP.restart(); - return found>index ? data.substring(strIndex[0], strIndex[1]) : ""; } +void cliGPS(void) { + Serial.println(); + Serial.println("================= GPS mode ================="); +} +void cliFCpass(void) { + Serial.println(); + Serial.println("=============== FC passthru ================"); +} + +#endif // ----------------------------------------------------------------------------- LoRa void sendMessage(planeData *outgoing) { while (!LoRa.beginPacket()) { } @@ -96,38 +171,91 @@ void sendMessage(planeData *outgoing) { void onReceive(int packetSize) { if (packetSize == 0) return; LoRa.readBytes((uint8_t *)&loraMsg, packetSize); - Serial.println(loraMsg.header); - if (!loraRX && String(loraMsg.header) == "ADS-RC") { // new plane data + cliLog(loraMsg.header); + if (loraMsg.header == cfg.loraHeader) { // new plane data loraRX = 1; pdIn = loraMsg; - Serial.println("new packet"); + cliLog("New air packet"); + bool found = 0; + size_t free = 0; + for (size_t i = 0; i <= 4; i++) { + if (pds[i].pd.loraAddress == pdIn.loraAddress ) { // update plane + pds[i].pd = pdIn; + pds[i].waypointNumber = i+1; + pds[i].lastUpdate = millis(); + found = 1; + cliLog("UAV DB update POI #" + String(i+1)); + } + if (!free && pds[i].waypointNumber == 0) free = i; // find free slot + } + if (!found) { // if not there put it in free slot + pds[free].waypointNumber = free+1; + pds[free].pd = pdIn; + pds[free].lastUpdate = millis(); + cliLog("UAV DB new POI #" + String(free+1)); + free = 0; + } } } +int moving = 0; void sendFakePlanes () { - for (size_t i = 0; i <= 4; i++) { - String("ADS-RC").toCharArray(fakepd.header,7); - fakepd.loraAddress = (char)i+1; - String("Testplane #").toCharArray(fakepd.planeName,20); - fakepd.armState= 1; - fakepd.gps.lat = 50.1006770 * 10000000; - fakepd.gps.lon = 8.7613380 * 10000000; - fakepd.gps.alt = 100; - fakepd.gps.groundSpeed = 50; - sendMessage(&fakepd); - delay(1000); - } + cliLog("Sending fake UAVs via radio ..."); + String("ADS-RC").toCharArray(fakepd.header,7); + fakepd.loraAddress = (char)1; + String("Testplane #1").toCharArray(fakepd.planeName,20); + fakepd.armState= 1; + fakepd.gps.lat = 50.1006770 * 10000000 + (1000 * moving); + fakepd.gps.lon = 8.7613380 * 10000000; + fakepd.gps.alt = 100; + fakepd.gps.groundSpeed = 50; + sendMessage(&fakepd); + delay(300); + fakepd.loraAddress = (char)2; + String("Testplane #2").toCharArray(fakepd.planeName,20); + fakepd.armState= 1; + fakepd.gps.lat = 50.100627 * 10000000 + (1000 * moving); + fakepd.gps.lon = 8.762765 * 10000000; + sendMessage(&fakepd); + delay(300); + fakepd.loraAddress = (char)3; + String("Testplane #3").toCharArray(fakepd.planeName,20); + fakepd.armState= 1; + fakepd.gps.lat = 50.100679 * 10000000; + fakepd.gps.lon = 8.762159 * 10000000 + (1000 * moving); + sendMessage(&fakepd); + delay(300); + fakepd.loraAddress = (char)4; + String("Testplane #4").toCharArray(fakepd.planeName,20); + fakepd.armState= 1; + fakepd.gps.lat = 50.099836 * 10000000; + fakepd.gps.lon = 8.762406 * 10000000 + (1000 * moving); + sendMessage(&fakepd); + delay(300); + fakepd.loraAddress = (char)5; + String("Testplane #5").toCharArray(fakepd.planeName,20); + fakepd.armState= 1; + fakepd.gps.lat = 50.1006770 * 10000000 + (1000 * moving); + fakepd.gps.lon = 8.762406 * 10000000; + sendMessage(&fakepd); + delay(300); + cliLog("Fake UAVs sent."); + moving++; } void initLora() { - display.drawString (0, 24, "LORA"); + #ifdef RADARESP32 + display.drawString (0, 8, "LORA"); display.display(); - pd.loraAddress = loraAddress; - String("ADS-RC").toCharArray(pd.header,7); - //pd.header = "ADS-RC"; + #endif + cliLog("Starting radio ..."); + pd.loraAddress = cfg.loraAddress; + String(cfg.loraHeader).toCharArray(pd.header,7); SPI.begin(5, 19, 27, 18); LoRa.setPins(SS,RST,DI0); if (!LoRa.begin(BAND)) { - Serial.println("Starting LoRa failed!"); - display.drawString (94, 24, "FAIL"); + cliLog("Radio start failed!"); + #ifdef RADARESP32 + display.drawString (94, 8, "FAIL"); + #endif while (1); } LoRa.sleep(); @@ -138,11 +266,15 @@ void initLora() { LoRa.onReceive(onReceive); LoRa.enableCrc(); LoRa.receive(); - Serial.print("- OK"); - display.drawString (100, 24, "OK"); + cliLog("Radio started."); + #ifdef RADARESP32 + display.drawString (100, 8, "OK"); display.display(); + #endif } // ----------------------------------------------------------------------------- Display +#ifdef RADARESP32 + void drawDisplay () { display.clear(); display.setTextAlignment (TEXT_ALIGN_LEFT); @@ -152,7 +284,7 @@ void drawDisplay () { display.drawString (106,54, "RX"); display.drawXbm(98, 55, 8, 8, loraTX ? activeSymbol : inactiveSymbol); - display.drawXbm(120, 55, 8, 8, loraRXd ? activeSymbol : inactiveSymbol); + display.drawXbm(120, 55, 8, 8, loraRX ? activeSymbol : inactiveSymbol); for (size_t i = 0; i <=4 ; i++) { if (pds[i].waypointNumber != 0) display.drawString (0,i*8, pds[i].pd.planeName); @@ -168,7 +300,7 @@ void drawDisplay () { } void initDisplay () { - Serial.print("Display "); + cliLog("Starting display ..."); pinMode (16, OUTPUT); pinMode (2, OUTPUT); digitalWrite (16, LOW); // set GPIO16 low to reset OLED @@ -181,20 +313,21 @@ void initDisplay () { display.drawString (0, 0, "DISPLAY"); display.drawString (100, 0, "OK"); display.display(); - Serial.println("- OK"); + cliLog("Display started!"); } +#endif // ----------------------------------------------------------------------------- MSP and FC void getPlanetArmed () { uint32_t planeModes; msp.getActiveModes(&planeModes); - Serial.print("Arm State: "); - Serial.println(bitRead(planeModes,0)); + //Serial.print("Arm State: "); + //Serial.println(bitRead(planeModes,0)); } void getPlaneGPS () { if (msp.request(MSP_RAW_GPS, &pd.gps, sizeof(pd.gps))) { - Serial.println(pd.gps.fixType); - Serial.println(pd.gps.numSat); + //Serial.println(pd.gps.fixType); + //Serial.println(pd.gps.numSat); } } @@ -202,7 +335,6 @@ void getPlaneData () { String("No FC").toCharArray(pd.planeName,20); if (msp.request(10, &pd.planeName, sizeof(pd.planeName))) { //Serial.println(pd.planeName); - } if (msp.request(2, &planeFC, sizeof(planeFC))) { //Serial.println(planeFC); @@ -211,7 +343,6 @@ void getPlaneData () { void planeSetWP () { msp_set_wp_t wp; - for (size_t i = 0; i <= 4; i++) { if (pds[i].waypointNumber != 0) { wp.waypointNumber = pds[i].waypointNumber; @@ -228,11 +359,27 @@ void planeSetWP () { } else break; } + cliLog("POIs sent to FC."); //msp.command(MSP_SET_WP, &wp, sizeof(wp)); } - void planeFakeWP () { +void planeFakeWPv2 () { + msp_raw_planes_t wp; + wp.id = 1; + wp.arm_state = 1; + wp.lat = 50.1006770 * 10000000; + wp.lon = 8.7613380 * 10000000; + wp.alt = 100; + wp.heading = 0; + wp.speed = 0; + wp.callsign0 = 'D'; + wp.callsign1 = 'A'; + wp.callsign2 = 'N'; + msp.commandv2(MSP2_ESP32, &wp, sizeof(wp)); +} + +void planeFakeWP () { msp_set_wp_t wp; - wp.waypointNumber = 1; +/* wp.waypointNumber = 1; wp.action = MSP_NAV_STATUS_WAYPOINT_ACTION_WAYPOINT; wp.lat = 50.1006770 * 10000000; wp.lon = 8.7613380 * 10000000; @@ -266,110 +413,117 @@ void planeSetWP () { wp.action = MSP_NAV_STATUS_WAYPOINT_ACTION_WAYPOINT; wp.lat = 50.100547 * 10000000; wp.lon = 8.764052 * 10000000; - wp.alt = 400; + wp.alt = 0; wp.p1 = 500; wp.p2 = 0; wp.p3 = 0; wp.flag = 0; msp.command(MSP_SET_WP, &wp, sizeof(wp)); - - wp.waypointNumber = 5; +*/ + wp.waypointNumber = 1; wp.action = MSP_NAV_STATUS_WAYPOINT_ACTION_WAYPOINT; wp.lat = 50.100306 * 10000000; wp.lon = 8.760833 * 10000000; - wp.alt = 500; + wp.alt = 0; wp.p1 = 1000; wp.p2 = 0; wp.p3 = 0; wp.flag = 0xa5; msp.command(MSP_SET_WP, &wp, sizeof(wp)); - + cliLog("Fake POIs sent to FC."); } void initMSP () { - Serial.print("MSP "); - display.drawString (0, 8, "MSP "); + cliLog("Starting MSP ..."); + #ifdef RADARESP32 + display.drawString (0, 16, "MSP "); display.display(); + #endif delay(200); Serial1.begin(115200, SERIAL_8N1, rxPin , txPin); msp.begin(Serial1); - Serial.println("- OK"); - display.drawString (100, 8, "OK"); + cliLog("MSP started!"); + #ifdef RADARESP32 + display.drawString (100, 16, "OK"); display.display(); - Serial.print("FC "); - display.drawString (0, 16, "FC "); + display.drawString (0, 24, "FC "); display.display(); + #endif + cliLog("Waiting for FC to start ..."); delay(2000); getPlaneData(); getPlanetArmed(); getPlaneGPS(); - Serial.print("- "); - Serial.println(planeFC); - display.drawString (100, 16, planeFC); + cliLog("FC detected: " + String(planeFC)); + #ifdef RADARESP32 + display.drawString (100, 24, planeFC); display.display(); + #endif } + // ----------------------------------------------------------------------------- main init void setup() { + + cli.RegisterCmd("status",&cliStatus); + cli.RegisterCmd("help",&cliHelp); + cli.RegisterCmd("config",&cliConfig); + cli.RegisterCmd("debug",&cliDebug); + cli.RegisterCmd("reboot",&cliReboot); Serial.begin(115200); + initConfig(); initDisplay(); + initLora(); + delay(1500); initMSP(); - delay(2000); + delay(1000); for (size_t i = 0; i <= 4; i++) { pds[i].pd.loraAddress= 0x00; } - -sendFakePlanes(); + booted = 1; } // ----------------------------------------------------------------------------- main loop void loop() { - if (loraRX) { - - bool found = 0; - size_t free = 0; - for (size_t i = 0; i <= 4; i++) { - if (pds[i].pd.loraAddress == pdIn.loraAddress ) { // update plane - pds[i].pd = pdIn; - pds[i].waypointNumber = i+1; - pds[i].lastUpdate = millis(); - found = 1; - Serial.println("update"); - } - if (!free && pds[i].waypointNumber == 0) free = i; // find free slot - } - if (!found) { // if not there put it in free slot - pds[free].waypointNumber = free+1; - pds[free].pd = pdIn; - pds[free].lastUpdate = millis(); - Serial.println("new"); - } - loraRXd = 1; - loraRX = 0; - } - - if (millis() - displayLastTime > displayInterval) { + if (millis() - displayLastTime > cfg.intervalDisplay) { + #ifdef RADARESP32 + drawDisplay(); + #endif for (size_t i = 0; i <= 4; i++) { - if (millis() - pds[i].lastUpdate > 500000) { // plane timeout + if (pds[i].pd.loraAddress != 0 && millis() - pds[i].lastUpdate > cfg.uavTimeout*1000 ) { // plane timeout pds[i].waypointNumber = 0; pds[i].pd.loraAddress = 0; + cliLog("UAV DB delete POI #" + String(i+1)); } } - drawDisplay(); loraTX = 0; - loraRXd = 0; + loraRX = 0; displayLastTime = millis(); } + if (millis() - pdLastTime > cfg.intervalStatus) { + getPlaneData(); + getPlanetArmed(); + if (!pd.armState) { + loraTX = 1; + sendMessage(&pd); + LoRa.receive(); + } + pdLastTime = millis(); + } - if (millis() - sendLastTime > sendInterval) { - getPlaneGPS(); - loraTX = 1; - sendMessage(&pd); - LoRa.receive(); - - //planeSetWP(); - planeFakeWP(); + if (millis() - sendLastTime > cfg.intervalSend + random(0, 20)) { + cli.Run(); + if (pd.armState) { + getPlaneGPS(); + loraTX = 1; + sendMessage(&pd); + LoRa.receive(); + } + if (cfg.debugFakePlanes) sendFakePlanes(); + if (pd.armState) planeSetWP(); + if (cfg.debugFakeWPs) planeFakeWP(); + //planeFakeWPv2(); sendLastTime = millis(); } } diff --git a/src/main.h b/src/main.h new file mode 100644 index 0000000..960ade0 --- /dev/null +++ b/src/main.h @@ -0,0 +1,55 @@ +struct config { + uint8_t configVersion; + char loraHeader[7]; // protocol identifier + uint8_t loraAddress; // local lora address + uint32_t loraFrequency; // 433E6, 868E6, 915E6 + uint32_t loraBandwidth; // 250000 bps + uint8_t loraCodingRate4; // 6? + uint8_t loraSpreadingFactor; // 7? + uint16_t intervalSend; // in ms + random + uint16_t intervalDisplay; // in ms + uint16_t intervalStatus; // in ms + uint8_t uavTimeout; // in sec + uint8_t mspTX; // pin for msp serial TX + uint8_t mspRX; // pin for msp serial RX + uint8_t mspPOI; // POI type: 1 (Wayponit), 2 (Plane) + + bool debugOutput; + bool debugFakeWPs; + bool debugFakePlanes; + bool debugFakeMoving; +}; + +struct planeData { + char header[7]; + byte loraAddress; + char planeName[20]; + bool armState; + msp_raw_gps_t gps; +}; +struct planesData { + uint8_t waypointNumber; + long lastUpdate; + planeData pd; +}; +const uint8_t activeSymbol[] PROGMEM = { + B00000000, + B00000000, + B00011000, + B00100100, + B01000010, + B01000010, + B00100100, + B00011000 +}; + +const uint8_t inactiveSymbol[] PROGMEM = { + B00000000, + B00000000, + B00000000, + B00000000, + B00011000, + B00011000, + B00000000, + B00000000 +};