From 703b51ea49590c71668fc414496c036822aff171 Mon Sep 17 00:00:00 2001 From: Olivier C Date: Sun, 14 Apr 2019 22:10:07 +0200 Subject: [PATCH 01/17] Barebone --- src/lib/ESPAsyncWiFiManager.cpp | 1112 --------------------------- src/lib/ESPAsyncWiFiManager.h | 263 ------- src/lib/MSP.h | 5 +- src/main.cpp | 1237 +++++++++++-------------------- src/main.h | 92 +-- 5 files changed, 447 insertions(+), 2262 deletions(-) delete mode 100755 src/lib/ESPAsyncWiFiManager.cpp delete mode 100755 src/lib/ESPAsyncWiFiManager.h diff --git a/src/lib/ESPAsyncWiFiManager.cpp b/src/lib/ESPAsyncWiFiManager.cpp deleted file mode 100755 index 7b728ad..0000000 --- a/src/lib/ESPAsyncWiFiManager.cpp +++ /dev/null @@ -1,1112 +0,0 @@ -/************************************************************** - AsyncWiFiManager is a library for the ESP8266/Arduino platform - (https://github.com/esp8266/Arduino) to enable easy - configuration and reconfiguration of WiFi credentials using a Captive Portal - inspired by: - http://www.esp8266.com/viewtopic.php?f=29&t=2520 - https://github.com/chriscook8/esp-arduino-apboot - https://github.com/esp8266/Arduino/tree/esp8266/hardware/esp8266com/esp8266/libraries/DNSServer/examples/CaptivePortalAdvanced - Built by AlexT https://github.com/tzapu - Ported to Async Web Server by https://github.com/alanswx - Licensed under MIT license - **************************************************************/ - -#include "lib/ESPAsyncWiFiManager.h" - -AsyncWiFiManagerParameter::AsyncWiFiManagerParameter(const char *custom) { - _id = NULL; - _placeholder = NULL; - _length = 0; - _value = NULL; - - _customHTML = custom; -} - -AsyncWiFiManagerParameter::AsyncWiFiManagerParameter(const char *id, const char *placeholder, const char *defaultValue, int length) { - init(id, placeholder, defaultValue, length, ""); -} - -AsyncWiFiManagerParameter::AsyncWiFiManagerParameter(const char *id, const char *placeholder, const char *defaultValue, int length, const char *custom) { - init(id, placeholder, defaultValue, length, custom); -} - -void AsyncWiFiManagerParameter::init(const char *id, const char *placeholder, const char *defaultValue, int length, const char *custom) { - _id = id; - _placeholder = placeholder; - _length = length; - _value = new char[length + 1]; - for (int i = 0; i < length; i++) { - _value[i] = 0; - } - if (defaultValue != NULL) { - strncpy(_value, defaultValue, length); - } - - _customHTML = custom; -} - -const char* AsyncWiFiManagerParameter::getValue() { - return _value; -} -const char* AsyncWiFiManagerParameter::getID() { - return _id; -} -const char* AsyncWiFiManagerParameter::getPlaceholder() { - return _placeholder; -} -int AsyncWiFiManagerParameter::getValueLength() { - return _length; -} -const char* AsyncWiFiManagerParameter::getCustomHTML() { - return _customHTML; -} - -#ifdef USE_EADNS -AsyncWiFiManager::AsyncWiFiManager(AsyncWebServer *server, AsyncDNSServer *dns) :server(server), dnsServer(dns) { -#else -AsyncWiFiManager::AsyncWiFiManager(AsyncWebServer *server, DNSServer *dns) :server(server), dnsServer(dns) { -#endif - wifiSSIDs = NULL; - wifiSSIDscan=true; - _modeless=false; - shouldscan=true; -} - -void AsyncWiFiManager::addParameter(AsyncWiFiManagerParameter *p) { - _params[_paramsCount] = p; - _paramsCount++; - DEBUG_WM("Adding parameter"); - DEBUG_WM(p->getID()); -} - -void AsyncWiFiManager::setupConfigPortal() { - // dnsServer.reset(new DNSServer()); - // server.reset(new ESP8266WebServer(80)); - server->reset(); - - DEBUG_WM(F("")); - _configPortalStart = millis(); - - DEBUG_WM(F("Configuring access point... ")); - DEBUG_WM(_apName); - if (_apPassword != NULL) { - if (strlen(_apPassword) < 8 || strlen(_apPassword) > 63) { - // fail passphrase to short or long! - DEBUG_WM(F("Invalid AccessPoint password. Ignoring")); - _apPassword = NULL; - } - DEBUG_WM(_apPassword); - } - - //optional soft ip config - if (_ap_static_ip) { - DEBUG_WM(F("Custom AP IP/GW/Subnet")); - WiFi.softAPConfig(_ap_static_ip, _ap_static_gw, _ap_static_sn); - } - - if (_apPassword != NULL) { - WiFi.softAP(_apName, _apPassword);//password option - } else { - WiFi.softAP(_apName); - } - - delay(500); // Without delay I've seen the IP address blank - DEBUG_WM(F("AP IP address: ")); - DEBUG_WM(WiFi.softAPIP()); - - /* Setup the DNS server redirecting all the domains to the apIP */ - #ifdef USE_EADNS - dnsServer->setErrorReplyCode(AsyncDNSReplyCode::NoError); - #else - dnsServer->setErrorReplyCode(DNSReplyCode::NoError); - #endif - dnsServer->start(DNS_PORT, "*", WiFi.softAPIP()); - - setInfo(); - - /* Setup web pages: root, wifi config pages, SO captive portal detectors and not found. */ - server->on("/", std::bind(&AsyncWiFiManager::handleRoot, this,std::placeholders::_1)).setFilter(ON_AP_FILTER); - server->on("/wifi", std::bind(&AsyncWiFiManager::handleWifi, this, std::placeholders::_1,true)).setFilter(ON_AP_FILTER); - server->on("/0wifi", std::bind(&AsyncWiFiManager::handleWifi, this,std::placeholders::_1, false)).setFilter(ON_AP_FILTER); - server->on("/wifisave", std::bind(&AsyncWiFiManager::handleWifiSave,this,std::placeholders::_1)).setFilter(ON_AP_FILTER); - server->on("/i", std::bind(&AsyncWiFiManager::handleInfo,this, std::placeholders::_1)).setFilter(ON_AP_FILTER); - server->on("/r", std::bind(&AsyncWiFiManager::handleReset, this,std::placeholders::_1)).setFilter(ON_AP_FILTER); - //server->on("/generate_204", std::bind(&AsyncWiFiManager::handle204, this)); //Android/Chrome OS captive portal check. - server->on("/fwlink", std::bind(&AsyncWiFiManager::handleRoot, this,std::placeholders::_1)).setFilter(ON_AP_FILTER); //Microsoft captive portal. Maybe not needed. Might be handled by notFound handler. - server->onNotFound (std::bind(&AsyncWiFiManager::handleNotFound,this,std::placeholders::_1)); - server->begin(); // Web server start - DEBUG_WM(F("HTTP server started")); - -} - -static const char HEX_CHAR_ARRAY[17] = "0123456789ABCDEF"; -/** -* convert char array (hex values) to readable string by seperator -* buf: buffer to convert -* length: data length -* strSeperator seperator between each hex value -* return: formated value as String -*/ -static String byteToHexString(uint8_t* buf, uint8_t length, String strSeperator="-") { - String dataString = ""; - for (uint8_t i = 0; i < length; i++) { - byte v = buf[i] / 16; - byte w = buf[i] % 16; - if (i>0) { - dataString += strSeperator; - } - dataString += String(HEX_CHAR_ARRAY[v]); - dataString += String(HEX_CHAR_ARRAY[w]); - } - dataString.toUpperCase(); - return dataString; -} // byteToHexString - -#if !defined(ESP8266) -String getESP32ChipID() { - uint64_t chipid; - chipid=ESP.getEfuseMac();//The chip ID is essentially its MAC address(length: 6 bytes). - int chipid_size = 6; - uint8_t chipid_arr[chipid_size]; - for (uint8_t i=0; i < chipid_size; i++) { - chipid_arr[i] = (chipid >> (8 * i)) & 0xff; - } - return byteToHexString(chipid_arr, chipid_size, ""); -} -#endif - -boolean AsyncWiFiManager::autoConnect() { - String ssid = "ESP"; - #if defined(ESP8266) - ssid += String(ESP.getChipId()); - #else - ssid += getESP32ChipID(); - #endif - return autoConnect(ssid.c_str(), NULL); -} - -boolean AsyncWiFiManager::autoConnect(char const *apName, char const *apPassword) { - DEBUG_WM(F("")); - DEBUG_WM(F("AutoConnect")); - - // read eeprom for ssid and pass - //String ssid = getSSID(); - //String pass = getPassword(); - - // attempt to connect; should it fail, fall back to AP - WiFi.mode(WIFI_STA); - - if (connectWifi("", "") == WL_CONNECTED) { - DEBUG_WM(F("IP Address:")); - DEBUG_WM(WiFi.localIP()); - //connected - return true; - } - - return startConfigPortal(apName, apPassword); -} - - -String AsyncWiFiManager::networkListAsString() -{ - String pager ; - //display networks in page - for (int i = 0; i < wifiSSIDCount; i++) { - if (wifiSSIDs[i].duplicate == true) continue; // skip dups - int quality = getRSSIasQuality(wifiSSIDs[i].RSSI); - - if (_minimumQuality == -1 || _minimumQuality < quality) { - String item = FPSTR(HTTP_ITEM); - String rssiQ; - rssiQ += quality; - item.replace("{v}", wifiSSIDs[i].SSID); - item.replace("{r}", rssiQ); -#if defined(ESP8266) - if (wifiSSIDs[i].encryptionType != ENC_TYPE_NONE) { -#else - if (wifiSSIDs[i].encryptionType != WIFI_AUTH_OPEN) { -#endif - item.replace("{i}", "l"); - } else { - item.replace("{i}", ""); - } - pager += item; - - } else { - DEBUG_WM(F("Skipping due to quality")); - } - - } - return pager; -} - -String AsyncWiFiManager::scanModal() -{ - shouldscan=true; - scan(); - String pager=networkListAsString(); - return pager; -} - -void AsyncWiFiManager::scan() -{ - if (!shouldscan) return; - DEBUG_WM(F("About to scan()")); - if (wifiSSIDscan) - { - delay(100); - } - - if (wifiSSIDscan) - { - int n = WiFi.scanNetworks(); - DEBUG_WM(F("Scan done")); - if (n == 0) { - DEBUG_WM(F("No networks found")); - // page += F("No networks found. Refresh to scan again."); - } else { - - - if (wifiSSIDscan) - { - /* WE SHOULD MOVE THIS IN PLACE ATOMICALLY */ - if (wifiSSIDs) delete [] wifiSSIDs; - wifiSSIDs = new WiFiResult[n]; - wifiSSIDCount = n; - - if (n>0) - shouldscan=false; - - for (int i=0;i wifiSSIDs[i].RSSI) { - std::swap(wifiSSIDs[i], wifiSSIDs[j]); - } - } - } - - - // remove duplicates ( must be RSSI sorted ) - if (_removeDuplicateAPs) { - String cssid; - for (int i = 0; i < n; i++) { - if (wifiSSIDs[i].duplicate == true) continue; - cssid = wifiSSIDs[i].SSID; - for (int j = i + 1; j < n; j++) { - if (cssid == wifiSSIDs[j].SSID) { - DEBUG_WM("DUP AP: " +wifiSSIDs[j].SSID); - wifiSSIDs[j].duplicate=true; // set dup aps to NULL - } - } - } - } - - } - } - } -} - - -void AsyncWiFiManager::startConfigPortalModeless(char const *apName, char const *apPassword) { - - _modeless =true; - _apName = apName; - _apPassword = apPassword; - - /* - AJS - do we want this? - - */ - - //setup AP - WiFi.mode(WIFI_AP_STA); - DEBUG_WM("SET AP STA"); - - // try to connect - if (connectWifi("", "") == WL_CONNECTED) { - DEBUG_WM(F("IP Address:")); - DEBUG_WM(WiFi.localIP()); - //connected - // call the callback! - _savecallback(); - - } - - - - //notify we entered AP mode - if ( _apcallback != NULL) { - _apcallback(this); - } - - connect = false; - setupConfigPortal(); - scannow= -1 ; - -} - -void AsyncWiFiManager::loop(){ - safeLoop(); - criticalLoop(); -} - -void AsyncWiFiManager::setInfo() { - if (needInfo) { - pager = infoAsString(); - wifiStatus = WiFi.status(); - needInfo = false; - } -} - -/** - * Anything that accesses WiFi, ESP or EEPROM goes here - */ -void AsyncWiFiManager::criticalLoop(){ - if (_modeless) - { - - if ( scannow==-1 || millis() > scannow + 60000) - { - - scan(); - scannow= millis() ; - } - if (connect) { - connect = false; - //delay(2000); - DEBUG_WM(F("Connecting to new AP")); - - // using user-provided _ssid, _pass in place of system-stored ssid and pass - if (connectWifi(_ssid, _pass) != WL_CONNECTED) { - DEBUG_WM(F("Failed to connect.")); - } else { - //connected - // alanswx - should we have a config to decide if we should shut down AP? - // WiFi.mode(WIFI_STA); - //notify that configuration has changed and any optional parameters should be saved - if ( _savecallback != NULL) { - //todo: check if any custom parameters actually exist, and check if they really changed maybe - _savecallback(); - } - - return; - } - - if (_shouldBreakAfterConfig) { - //flag set to exit after config after trying to connect - //notify that configuration has changed and any optional parameters should be saved - if ( _savecallback != NULL) { - //todo: check if any custom parameters actually exist, and check if they really changed maybe - _savecallback(); - } - } - } - } -} - -/* - * Anything that doesn't access WiFi, ESP or EEPROM can go here - */ -void AsyncWiFiManager::safeLoop(){ - #ifndef USE_EADNS - dnsServer->processNextRequest(); - #endif -} - -boolean AsyncWiFiManager::startConfigPortal(char const *apName, char const *apPassword) { - //setup AP - WiFi.mode(WIFI_AP_STA); - DEBUG_WM("SET AP STA"); - - _apName = apName; - _apPassword = apPassword; - - //notify we entered AP mode - if ( _apcallback != NULL) { - _apcallback(this); - } - - connect = false; - setupConfigPortal(); - scannow= -1 ; - while (_configPortalTimeout == 0 || millis() < _configPortalStart + _configPortalTimeout) { - //DNS - #ifndef USE_EADNS - dnsServer->processNextRequest(); - #endif - - // - // we should do a scan every so often here - // - if ( millis() > scannow + 10000) - { - DEBUG_WM(F("About to scan()")); - shouldscan=true; // since we are modal, we can scan every time - scan(); - scannow= millis() ; - } - - - if (connect) { - connect = false; - delay(2000); - DEBUG_WM(F("Connecting to new AP")); - - // using user-provided _ssid, _pass in place of system-stored ssid and pass - if (connectWifi(_ssid, _pass) != WL_CONNECTED) { - DEBUG_WM(F("Failed to connect.")); - } else { - //connected - WiFi.mode(WIFI_STA); - //notify that configuration has changed and any optional parameters should be saved - if ( _savecallback != NULL) { - //todo: check if any custom parameters actually exist, and check if they really changed maybe - _savecallback(); - } - break; - } - - if (_shouldBreakAfterConfig) { - //flag set to exit after config after trying to connect - //notify that configuration has changed and any optional parameters should be saved - if ( _savecallback != NULL) { - //todo: check if any custom parameters actually exist, and check if they really changed maybe - _savecallback(); - } - break; - } - } - yield(); - } - - server->reset(); - #ifdef USE_EADNS - *dnsServer=AsyncDNSServer(); - #else - *dnsServer=DNSServer(); - #endif - - return WiFi.status() == WL_CONNECTED; -} - - -int AsyncWiFiManager::connectWifi(String ssid, String pass) { - DEBUG_WM(F("Connecting as wifi client...")); - - // check if we've got static_ip settings, if we do, use those. - if (_sta_static_ip) { - DEBUG_WM(F("Custom STA IP/GW/Subnet/DNS")); - WiFi.config(_sta_static_ip, _sta_static_gw, _sta_static_sn, _sta_static_dns1, _sta_static_dns2); - DEBUG_WM(WiFi.localIP()); - } - //fix for auto connect racing issue - // if (WiFi.status() == WL_CONNECTED) { - // DEBUG_WM("Already connected. Bailing out."); - // return WL_CONNECTED; - // } - //check if we have ssid and pass and force those, if not, try with last saved values - if (ssid != "") { - #if defined(ESP8266) - //trying to fix connection in progress hanging - ETS_UART_INTR_DISABLE(); - wifi_station_disconnect(); - ETS_UART_INTR_ENABLE(); - #else - WiFi.disconnect(false); - #endif - - WiFi.begin(ssid.c_str(), pass.c_str()); - } else { - if (WiFi.SSID().length() > 0) { - DEBUG_WM("Using last saved values, should be faster"); -#if defined(ESP8266) - //trying to fix connection in progress hanging - ETS_UART_INTR_DISABLE(); - wifi_station_disconnect(); - ETS_UART_INTR_ENABLE(); -#else - WiFi.disconnect(false); -#endif - - WiFi.begin(); - } else { - DEBUG_WM("Try to connect with saved credentials"); - WiFi.begin(); - } - } - - int connRes = waitForConnectResult(); - DEBUG_WM ("Connection result: "); - DEBUG_WM ( connRes ); - //not connected, WPS enabled, no pass - first attempt -#ifdef NO_EXTRA_4K_HEAP - if (_tryWPS && connRes != WL_CONNECTED && pass == "") { - startWPS(); - //should be connected at the end of WPS - connRes = waitForConnectResult(); - } -#endif - needInfo = true; - setInfo(); - return connRes; -} - -uint8_t AsyncWiFiManager::waitForConnectResult() { - if (_connectTimeout == 0) { - return WiFi.waitForConnectResult(); - } else { - DEBUG_WM (F("Waiting for connection result with time out")); - unsigned long start = millis(); - boolean keepConnecting = true; - uint8_t status; - while (keepConnecting) { - status = WiFi.status(); - if (millis() > start + _connectTimeout) { - keepConnecting = false; - DEBUG_WM (F("Connection timed out")); - } - if (status == WL_CONNECTED || status == WL_CONNECT_FAILED) { - keepConnecting = false; - } - delay(100); - } - return status; - } -} -#ifdef NO_EXTRA_4K_HEAP -void AsyncWiFiManager::startWPS() { - DEBUG_WM("START WPS"); -#if defined(ESP8266) - WiFi.beginWPSConfig(); -#else - //esp_wps_config_t config = WPS_CONFIG_INIT_DEFAULT(ESP_WPS_MODE); - esp_wps_config_t config = {}; - config.wps_type = ESP_WPS_MODE; - config.crypto_funcs = &g_wifi_default_wps_crypto_funcs; - strcpy(config.factory_info.manufacturer,"ESPRESSIF"); - strcpy(config.factory_info.model_number, "ESP32"); - strcpy(config.factory_info.model_name, "ESPRESSIF IOT"); - strcpy(config.factory_info.device_name,"ESP STATION"); - - esp_wifi_wps_enable(&config); - esp_wifi_wps_start(0); -#endif - DEBUG_WM("END WPS"); - -} -#endif -/* -String AsyncWiFiManager::getSSID() { -if (_ssid == "") { -DEBUG_WM(F("Reading SSID")); -_ssid = WiFi.SSID(); -DEBUG_WM(F("SSID: ")); -DEBUG_WM(_ssid); -} -return _ssid; -} - -String AsyncWiFiManager::getPassword() { -if (_pass == "") { -DEBUG_WM(F("Reading Password")); -_pass = WiFi.psk(); -DEBUG_WM("Password: " + _pass); -//DEBUG_WM(_pass); -} -return _pass; -} -*/ -String AsyncWiFiManager::getConfigPortalSSID() { - return _apName; -} - -void AsyncWiFiManager::resetSettings() { - DEBUG_WM(F("settings invalidated")); - DEBUG_WM(F("THIS MAY CAUSE AP NOT TO START UP PROPERLY. YOU NEED TO COMMENT IT OUT AFTER ERASING THE DATA.")); - WiFi.disconnect(true); - //delay(200); -} -void AsyncWiFiManager::setTimeout(unsigned long seconds) { - setConfigPortalTimeout(seconds); -} - -void AsyncWiFiManager::setConfigPortalTimeout(unsigned long seconds) { - _configPortalTimeout = seconds * 1000; -} - -void AsyncWiFiManager::setConnectTimeout(unsigned long seconds) { - _connectTimeout = seconds * 1000; -} - -void AsyncWiFiManager::setDebugOutput(boolean debug) { - _debug = debug; -} - -void AsyncWiFiManager::setAPStaticIPConfig(IPAddress ip, IPAddress gw, IPAddress sn) { - _ap_static_ip = ip; - _ap_static_gw = gw; - _ap_static_sn = sn; -} - -void AsyncWiFiManager::setSTAStaticIPConfig(IPAddress ip, IPAddress gw, IPAddress sn, IPAddress dns1, IPAddress dns2) { - _sta_static_ip = ip; - _sta_static_gw = gw; - _sta_static_sn = sn; - _sta_static_dns1 = dns1; - _sta_static_dns2 = dns2; -} - -void AsyncWiFiManager::setMinimumSignalQuality(int quality) { - _minimumQuality = quality; -} - -void AsyncWiFiManager::setBreakAfterConfig(boolean shouldBreak) { - _shouldBreakAfterConfig = shouldBreak; -} - -/** Handle root or redirect to captive portal */ -void AsyncWiFiManager::handleRoot(AsyncWebServerRequest *request) { - // AJS - maybe we should set a scan when we get to the root??? - // and only scan on demand? timer + on demand? plus a link to make it happen? - shouldscan=true; - scannow= -1 ; - DEBUG_WM(F("Handle root")); - if (captivePortal(request)) { // If captive portal redirect instead of displaying the page. - return; - } - - String page = FPSTR(WFM_HTTP_HEAD); - page.replace("{v}", "Options"); - page += FPSTR(HTTP_SCRIPT); - page += FPSTR(HTTP_STYLE); - page += _customHeadElement; - page += FPSTR(HTTP_HEAD_END); - page += "

"; - page += _apName; - page += "

"; - page += F("

AsyncWiFiManager

"); - page += FPSTR(HTTP_PORTAL_OPTIONS); - page += FPSTR(HTTP_END); - - request->send(200, "text/html", page); - -} - -/** Wifi config page handler */ -void AsyncWiFiManager::handleWifi(AsyncWebServerRequest *request,boolean scan) { - shouldscan=true; - scannow= -1 ; - - String page = FPSTR(WFM_HTTP_HEAD); - page.replace("{v}", "Config ESP"); - page += FPSTR(HTTP_SCRIPT); - page += FPSTR(HTTP_STYLE); - page += _customHeadElement; - page += FPSTR(HTTP_HEAD_END); - - if (scan) { - wifiSSIDscan=false; - - - - DEBUG_WM(F("Scan done")); - if (wifiSSIDCount==0) { - DEBUG_WM(F("No networks found")); - page += F("No networks found. Refresh to scan again."); - } else { - - - //display networks in page - String pager = networkListAsString(); - - page += pager; - page += "
"; - } - - } - wifiSSIDscan=true; - - page += FPSTR(HTTP_FORM_START); - char parLength[2]; - // add the extra parameters to the form - for (int i = 0; i < _paramsCount; i++) { - if (_params[i] == NULL) { - break; - } - - String pitem = FPSTR(HTTP_FORM_PARAM); - if (_params[i]->getID() != NULL) { - pitem.replace("{i}", _params[i]->getID()); - pitem.replace("{n}", _params[i]->getID()); - pitem.replace("{p}", _params[i]->getPlaceholder()); - snprintf(parLength, 2, "%d", _params[i]->getValueLength()); - pitem.replace("{l}", parLength); - pitem.replace("{v}", _params[i]->getValue()); - pitem.replace("{c}", _params[i]->getCustomHTML()); - } else { - pitem = _params[i]->getCustomHTML(); - } - - page += pitem; - } - if (_params[0] != NULL) { - page += "
"; - } - - if (_sta_static_ip) { - - String item = FPSTR(HTTP_FORM_PARAM); - item.replace("{i}", "ip"); - item.replace("{n}", "ip"); - item.replace("{p}", "Static IP"); - item.replace("{l}", "15"); - item.replace("{v}", _sta_static_ip.toString()); - - page += item; - - item = FPSTR(HTTP_FORM_PARAM); - item.replace("{i}", "gw"); - item.replace("{n}", "gw"); - item.replace("{p}", "Static Gateway"); - item.replace("{l}", "15"); - item.replace("{v}", _sta_static_gw.toString()); - - page += item; - - item = FPSTR(HTTP_FORM_PARAM); - item.replace("{i}", "sn"); - item.replace("{n}", "sn"); - item.replace("{p}", "Subnet"); - item.replace("{l}", "15"); - item.replace("{v}", _sta_static_sn.toString()); - - page += item; - - item = FPSTR(HTTP_FORM_PARAM); - item.replace("{i}", "dns1"); - item.replace("{n}", "dns1"); - item.replace("{p}", "DNS1"); - item.replace("{l}", "15"); - item.replace("{v}", _sta_static_dns1.toString()); - - page += item; - - item = FPSTR(HTTP_FORM_PARAM); - item.replace("{i}", "dns2"); - item.replace("{n}", "dns2"); - item.replace("{p}", "DNS2"); - item.replace("{l}", "15"); - item.replace("{v}", _sta_static_dns2.toString()); - - page += item; - - page += "
"; - } - - page += FPSTR(HTTP_FORM_END); - page += FPSTR(HTTP_SCAN_LINK); - - page += FPSTR(HTTP_END); - - request->send(200, "text/html", page); - - - DEBUG_WM(F("Sent config page")); -} - -/** Handle the WLAN save form and redirect to WLAN config page again */ -void AsyncWiFiManager::handleWifiSave(AsyncWebServerRequest *request) { - DEBUG_WM(F("WiFi save")); - - //SAVE/connect here - needInfo = true; - _ssid = request->arg("s").c_str(); - _pass = request->arg("p").c_str(); - - //parameters - for (int i = 0; i < _paramsCount; i++) { - if (_params[i] == NULL) { - break; - } - //read parameter - String value = request->arg(_params[i]->getID()).c_str(); - //store it in array - value.toCharArray(_params[i]->_value, _params[i]->_length); - DEBUG_WM(F("Parameter")); - DEBUG_WM(_params[i]->getID()); - DEBUG_WM(value); - } - - if (request->hasArg("ip")) { - DEBUG_WM(F("static ip")); - DEBUG_WM(request->arg("ip")); - //_sta_static_ip.fromString(request->arg("ip")); - String ip = request->arg("ip"); - optionalIPFromString(&_sta_static_ip, ip.c_str()); - } - if (request->hasArg("gw")) { - DEBUG_WM(F("static gateway")); - DEBUG_WM(request->arg("gw")); - String gw = request->arg("gw"); - optionalIPFromString(&_sta_static_gw, gw.c_str()); - } - if (request->hasArg("sn")) { - DEBUG_WM(F("static netmask")); - DEBUG_WM(request->arg("sn")); - String sn = request->arg("sn"); - optionalIPFromString(&_sta_static_sn, sn.c_str()); - } - if (request->hasArg("dns1")) { - DEBUG_WM(F("static DNS 1")); - DEBUG_WM(request->arg("dns1")); - String dns1 = request->arg("dns1"); - optionalIPFromString(&_sta_static_dns1, dns1.c_str()); - } - if (request->hasArg("dns2")) { - DEBUG_WM(F("static DNS 2")); - DEBUG_WM(request->arg("dns2")); - String dns2 = request->arg("dns2"); - optionalIPFromString(&_sta_static_dns2, dns2.c_str()); - } - - String page = FPSTR(WFM_HTTP_HEAD); - page.replace("{v}", "Credentials Saved"); - page += FPSTR(HTTP_SCRIPT); - page += FPSTR(HTTP_STYLE); - page += _customHeadElement; - page += F(""); - page += FPSTR(HTTP_HEAD_END); - page += FPSTR(HTTP_SAVED); - page += FPSTR(HTTP_END); - - request->send(200, "text/html", page); - - DEBUG_WM(F("Sent wifi save page")); - - connect = true; //signal ready to connect/reset -} - -/** Handle the info page */ -String AsyncWiFiManager::infoAsString() -{ - String page; - page += F("
Chip ID
"); -#if defined(ESP8266) - page += ESP.getChipId(); -#else - page += getESP32ChipID(); -#endif - page += F("
"); - page += F("
Flash Chip ID
"); -#if defined(ESP8266) - page += ESP.getFlashChipId(); -#else - page += F("N/A for ESP32"); -#endif - page += F("
"); - page += F("
IDE Flash Size
"); - page += ESP.getFlashChipSize(); - page += F(" bytes
"); - page += F("
Real Flash Size
"); -#if defined(ESP8266) - page += ESP.getFlashChipRealSize(); -#else - page += F("N/A for ESP32"); -#endif - page += F(" bytes
"); - page += F("
Soft AP IP
"); - page += WiFi.softAPIP().toString(); - page += F("
"); - page += F("
Soft AP MAC
"); - page += WiFi.softAPmacAddress(); - page += F("
"); - page += F("
Station SSID
"); - page += WiFi.SSID(); - page += F("
"); - page += F("
Station IP
"); - page += WiFi.localIP().toString(); - page += F("
"); - page += F("
Station MAC
"); - page += WiFi.macAddress(); - page += F("
"); - page += F(""); - return page; -} - -void AsyncWiFiManager::handleInfo(AsyncWebServerRequest *request) { - DEBUG_WM(F("Info")); - - String page = FPSTR(WFM_HTTP_HEAD); - page.replace("{v}", "Info"); - page += FPSTR(HTTP_SCRIPT); - page += FPSTR(HTTP_STYLE); - page += _customHeadElement; - if (connect==true) - page += F(""); - page += FPSTR(HTTP_HEAD_END); - page += F("
"); - if (connect==true) - { - page += F("
Trying to connect
"); - page += wifiStatus; - page += F("
"); - } - - page +=pager; - page += FPSTR(HTTP_END); - - request->send(200, "text/html", page); - - DEBUG_WM(F("Sent info page")); -} - -/** Handle the reset page */ -void AsyncWiFiManager::handleReset(AsyncWebServerRequest *request) { - DEBUG_WM(F("Reset")); - - String page = FPSTR(WFM_HTTP_HEAD); - page.replace("{v}", "Info"); - page += FPSTR(HTTP_SCRIPT); - page += FPSTR(HTTP_STYLE); - page += _customHeadElement; - page += FPSTR(HTTP_HEAD_END); - page += F("Module will reset in a few seconds."); - page += FPSTR(HTTP_END); - request->send(200, "text/html", page); - - DEBUG_WM(F("Sent reset page")); - delay(5000); - #if defined(ESP8266) - ESP.reset(); - #else - ESP.restart(); - #endif - delay(2000); -} - - - -//removed as mentioned here https://github.com/tzapu/AsyncWiFiManager/issues/114 -/*void AsyncWiFiManager::handle204(AsyncWebServerRequest *request) { -DEBUG_WM(F("204 No Response")); -request->sendHeader("Cache-Control", "no-cache, no-store, must-revalidate"); -request->sendHeader("Pragma", "no-cache"); -request->sendHeader("Expires", "-1"); -request->send ( 204, "text/plain", ""); - -}*/ - -void AsyncWiFiManager::handleNotFound(AsyncWebServerRequest *request) { - if (captivePortal(request)) { // If captive portal redirect instead of displaying the error page. - return; - } - String message = "File Not Found\n\n"; - message += "URI: "; - message += request->url(); - message += "\nMethod: "; - message += ( request->method() == HTTP_GET ) ? "GET" : "POST"; - message += "\nArguments: "; - message += request->args(); - message += "\n"; - - for ( uint8_t i = 0; i < request->args(); i++ ) { - message += " " + request->argName ( i ) + ": " + request->arg ( i ) + "\n"; - } - AsyncWebServerResponse *response = request->beginResponse(404,"text/plain",message); - response->addHeader("Cache-Control", "no-cache, no-store, must-revalidate"); - response->addHeader("Pragma", "no-cache"); - response->addHeader("Expires", "-1"); - request->send (response ); -} - - -/** Redirect to captive portal if we got a request for another domain. Return true in that case so the page handler do not try to handle the request again. */ -boolean AsyncWiFiManager::captivePortal(AsyncWebServerRequest *request) { - if (!isIp(request->host()) ) { - DEBUG_WM(F("Request redirected to captive portal")); - AsyncWebServerResponse *response = request->beginResponse(302,"text/plain",""); - response->addHeader("Location", String("http://") + toStringIp(request->client()->localIP())); - request->send ( response); - return true; - } - return false; -} - -//start up config portal callback -void AsyncWiFiManager::setAPCallback( void (*func)(AsyncWiFiManager* myAsyncWiFiManager) ) { - _apcallback = func; -} - -//start up save config callback -void AsyncWiFiManager::setSaveConfigCallback( void (*func)(void) ) { - _savecallback = func; -} - -//sets a custom element to add to head, like a new style tag -void AsyncWiFiManager::setCustomHeadElement(const char* element) { - _customHeadElement = element; -} - -//if this is true, remove duplicated Access Points - defaut true -void AsyncWiFiManager::setRemoveDuplicateAPs(boolean removeDuplicates) { - _removeDuplicateAPs = removeDuplicates; -} - - - -template -void AsyncWiFiManager::DEBUG_WM(Generic text) { - if (_debug) { - Serial.print("*WM: "); - Serial.println(text); - } -} - -int AsyncWiFiManager::getRSSIasQuality(int RSSI) { - int quality = 0; - - if (RSSI <= -100) { - quality = 0; - } else if (RSSI >= -50) { - quality = 100; - } else { - quality = 2 * (RSSI + 100); - } - return quality; -} - -/** Is this an IP? */ -boolean AsyncWiFiManager::isIp(String str) { - for (int i = 0; i < str.length(); i++) { - int c = str.charAt(i); - if (c != '.' && (c < '0' || c > '9')) { - return false; - } - } - return true; -} - -/** IP to String? */ -String AsyncWiFiManager::toStringIp(IPAddress ip) { - String res = ""; - for (int i = 0; i < 3; i++) { - res += String((ip >> (8 * i)) & 0xFF) + "."; - } - res += String(((ip >> 8 * 3)) & 0xFF); - return res; -} diff --git a/src/lib/ESPAsyncWiFiManager.h b/src/lib/ESPAsyncWiFiManager.h deleted file mode 100755 index 54c7ee4..0000000 --- a/src/lib/ESPAsyncWiFiManager.h +++ /dev/null @@ -1,263 +0,0 @@ -/************************************************************** - WiFiManager is a library for the ESP8266/Arduino platform - (https://github.com/esp8266/Arduino) to enable easy - configuration and reconfiguration of WiFi credentials using a Captive Portal - inspired by: - http://www.esp8266.com/viewtopic.php?f=29&t=2520 - https://github.com/chriscook8/esp-arduino-apboot - https://github.com/esp8266/Arduino/tree/esp8266/hardware/esp8266com/esp8266/libraries/DNSServer/examples/CaptivePortalAdvanced - Built by AlexT https://github.com/tzapu - Ported to Async Web Server by https://github.com/alanswx - Licensed under MIT license - **************************************************************/ - -#ifndef ESPAsyncWiFiManager_h -#define ESPAsyncWiFiManager_h - -#if defined(ESP8266) -#include //https://github.com/esp8266/Arduino -#else -#include -#include "esp_wps.h" -#define ESP_WPS_MODE WPS_TYPE_PBC -#endif -#include - -//#define USE_EADNS //Uncomment to use ESPAsyncDNSServer -#ifdef USE_EADNS -#include //https://github.com/devyte/ESPAsyncDNSServer - //https://github.com/me-no-dev/ESPAsyncUDP -#else -#include -#endif -#include - -#if defined(ESP8266) -extern "C" { - #include "user_interface.h" -} -#else -#include -#endif - -const char WFM_HTTP_HEAD[] PROGMEM = "{v}"; -const char HTTP_STYLE[] PROGMEM = ""; -const char HTTP_SCRIPT[] PROGMEM = ""; -const char HTTP_HEAD_END[] PROGMEM = "
"; -const char HTTP_PORTAL_OPTIONS[] PROGMEM = "



"; -const char HTTP_ITEM[] PROGMEM = "
{v} {r}%
"; -const char HTTP_FORM_START[] PROGMEM = "


"; -const char HTTP_FORM_PARAM[] PROGMEM = "
"; -const char HTTP_FORM_END[] PROGMEM = "
"; -const char HTTP_SCAN_LINK[] PROGMEM = "
Scan
"; -const char HTTP_SAVED[] PROGMEM = "
Credentials Saved
Trying to connect ESP to network.
If it fails reconnect to AP to try again
"; -const char HTTP_END[] PROGMEM = "
"; - -#define WIFI_MANAGER_MAX_PARAMS 10 - -class AsyncWiFiManagerParameter { -public: - AsyncWiFiManagerParameter(const char *custom); - AsyncWiFiManagerParameter(const char *id, const char *placeholder, const char *defaultValue, int length); - AsyncWiFiManagerParameter(const char *id, const char *placeholder, const char *defaultValue, int length, const char *custom); - - const char *getID(); - const char *getValue(); - const char *getPlaceholder(); - int getValueLength(); - const char *getCustomHTML(); -private: - const char *_id; - const char *_placeholder; - char *_value; - int _length; - const char *_customHTML; - - void init(const char *id, const char *placeholder, const char *defaultValue, int length, const char *custom); - - friend class AsyncWiFiManager; -}; - - -class WiFiResult -{ -public: - bool duplicate; - String SSID; - uint8_t encryptionType; - int32_t RSSI; - uint8_t* BSSID; - int32_t channel; - bool isHidden; - - WiFiResult() - { - } - - -}; - -class AsyncWiFiManager -{ -public: - #ifdef USE_EADNS - AsyncWiFiManager(AsyncWebServer * server, AsyncDNSServer *dns); - #else - AsyncWiFiManager(AsyncWebServer * server, DNSServer *dns); - #endif - - void scan(); - String scanModal(); - void loop(); - void safeLoop(); - void criticalLoop(); - String infoAsString(); - - boolean autoConnect(); - boolean autoConnect(char const *apName, char const *apPassword = NULL); - - //if you want to always start the config portal, without trying to connect first - boolean startConfigPortal(char const *apName, char const *apPassword = NULL); - void startConfigPortalModeless(char const *apName, char const *apPassword); - - // get the AP name of the config portal, so it can be used in the callback - String getConfigPortalSSID(); - - void resetSettings(); - - //sets timeout before webserver loop ends and exits even if there has been no setup. - //usefully for devices that failed to connect at some point and got stuck in a webserver loop - //in seconds setConfigPortalTimeout is a new name for setTimeout - void setConfigPortalTimeout(unsigned long seconds); - void setTimeout(unsigned long seconds); - - //sets timeout for which to attempt connecting, usefull if you get a lot of failed connects - void setConnectTimeout(unsigned long seconds); - - - void setDebugOutput(boolean debug); - //defaults to not showing anything under 8% signal quality if called - void setMinimumSignalQuality(int quality = 8); - //sets a custom ip /gateway /subnet configuration - void setAPStaticIPConfig(IPAddress ip, IPAddress gw, IPAddress sn); - //sets config for a static IP - void setSTAStaticIPConfig(IPAddress ip, IPAddress gw, IPAddress sn, IPAddress dns1=(uint32_t)0x00000000, IPAddress dns2=(uint32_t)0x00000000); - //called when AP mode and config portal is started - void setAPCallback( void (*func)(AsyncWiFiManager*) ); - //called when settings have been changed and connection was successful - void setSaveConfigCallback( void (*func)(void) ); - //adds a custom parameter - void addParameter(AsyncWiFiManagerParameter *p); - //if this is set, it will exit after config, even if connection is unsucessful. - void setBreakAfterConfig(boolean shouldBreak); - //if this is set, try WPS setup when starting (this will delay config portal for up to 2 mins) - //TODO - //if this is set, customise style - void setCustomHeadElement(const char* element); - //if this is true, remove duplicated Access Points - defaut true - void setRemoveDuplicateAPs(boolean removeDuplicates); - -private: - #ifdef USE_EADNS - AsyncDNSServer *dnsServer; - #else - DNSServer *dnsServer; - #endif - AsyncWebServer *server; - - - boolean _modeless; - int scannow; - int shouldscan; - boolean needInfo = true; - - //const int WM_DONE = 0; - //const int WM_WAIT = 10; - - //const String HTTP_HEAD = "{v}"; - - void setupConfigPortal(); -#ifdef NO_EXTRA_4K_HEAP - void startWPS(); -#endif - String pager; - wl_status_t wifiStatus; - const char* _apName = "no-net"; - const char* _apPassword = NULL; - String _ssid = ""; - String _pass = ""; - unsigned long _configPortalTimeout = 0; - unsigned long _connectTimeout = 0; - unsigned long _configPortalStart = 0; - - IPAddress _ap_static_ip; - IPAddress _ap_static_gw; - IPAddress _ap_static_sn; - IPAddress _sta_static_ip; - IPAddress _sta_static_gw; - IPAddress _sta_static_sn; - IPAddress _sta_static_dns1= (uint32_t)0x00000000; - IPAddress _sta_static_dns2= (uint32_t)0x00000000; - - int _paramsCount = 0; - int _minimumQuality = -1; - boolean _removeDuplicateAPs = true; - boolean _shouldBreakAfterConfig = false; -#ifdef NO_EXTRA_4K_HEAP - boolean _tryWPS = false; -#endif - const char* _customHeadElement = ""; - - //String getEEPROMString(int start, int len); - //void setEEPROMString(int start, int len, String string); - - int status = WL_IDLE_STATUS; - int connectWifi(String ssid, String pass); - uint8_t waitForConnectResult(); - void setInfo(); - - String networkListAsString(); - - void handleRoot(AsyncWebServerRequest *); - void handleWifi(AsyncWebServerRequest*,boolean scan); - void handleWifiSave(AsyncWebServerRequest*); - void handleInfo(AsyncWebServerRequest*); - void handleReset(AsyncWebServerRequest*); - void handleNotFound(AsyncWebServerRequest*); - void handle204(AsyncWebServerRequest*); - boolean captivePortal(AsyncWebServerRequest*); - - // DNS server - const byte DNS_PORT = 53; - - //helpers - int getRSSIasQuality(int RSSI); - boolean isIp(String str); - String toStringIp(IPAddress ip); - - boolean connect; - boolean _debug = true; - - WiFiResult *wifiSSIDs; - int wifiSSIDCount; - boolean wifiSSIDscan; - - void (*_apcallback)(AsyncWiFiManager*) = NULL; - void (*_savecallback)(void) = NULL; - - AsyncWiFiManagerParameter* _params[WIFI_MANAGER_MAX_PARAMS]; - - template - void DEBUG_WM(Generic text); - - template - auto optionalIPFromString(T *obj, const char *s) -> decltype( obj->fromString(s) ) { - return obj->fromString(s); - } - auto optionalIPFromString(...) -> bool { - DEBUG_WM("NO fromString METHOD ON IPAddress, you need ESP8266 core 2.1.0 or newer for Custom IP configuration to work."); - return false; - } -}; - -#endif diff --git a/src/lib/MSP.h b/src/lib/MSP.h index 871ef7c..659735e 100644 --- a/src/lib/MSP.h +++ b/src/lib/MSP.h @@ -224,8 +224,11 @@ struct msp_servo_configurations_t { } __attribute__ ((packed)); +/* #define MSP_MAX_SERVO_RULES (2 * MSP_MAX_SUPPORTED_SERVOS) + + // MSP_SERVO_MIX_RULES reply struct msp_servo_mix_rules_t { __attribute__ ((packed)) struct { @@ -237,7 +240,7 @@ struct msp_servo_mix_rules_t { uint8_t max; } mixRule[MSP_MAX_SERVO_RULES]; } __attribute__ ((packed)); - +*/ #define MSP_MAX_SUPPORTED_MOTORS 8 diff --git a/src/main.cpp b/src/main.cpp index de70b1d..29f94b7 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,8 +4,6 @@ #include #include #include -#include -using namespace simplecli; #include #define SCK 5 // GPIO5 - SX1278's SCK @@ -15,742 +13,321 @@ using namespace simplecli; #define RST 14 // GPIO14 - SX1278's RESET #define DI0 26 // GPIO26 - SX1278's IRQ (interrupt request) -#define CFGVER 24 // bump up to overwrite setting with new defaults -#define VERSION "A82" +#define VERSION "Z1" + // ----------------------------------------------------------------------------- global vars config cfg; MSP msp; -bool booted = 0; -Stream *serialConsole[1]; -int cNum = 0; -int displayPage = 0; -SSD1306 display (0x3c, 4, 15); - -long sendLastTime = 0; -long displayLastTime = 0; -long pdLastTime = 0; -long pickupTime = 0; + +// Stream *serialConsole[1]; + +int sys_display_page = 0; +SSD1306 display(0x3c, 4, 15); + +long sys_pickup_updated = 0; + +long lora_last_tx = 0; +long main_updated = 0; long currentUpdateTime = 0; -msp_analog_t fcanalog; // analog values from FC -msp_status_ex_t fcstatusex; // extended status from FC -msp_raw_gps_t homepos; // set on arm -planeData pd; // our uav data -planeData pdIn; // new air packet -planeData loraMsg; // incoming packet -planesData pds[5]; // uav db -planeData fakepd; // debugging plane -char planeFC[20]; // uav fc name -bool loraRX = 0; // display RX -bool loraTX = 0; // display TX -uint8_t loraSeqNum = 0; -int numPlanes = 0; -String rssi = "0"; -uint8_t pps = 0; -uint8_t ppsc = 0; -bool buttonState = 1; -bool buttonPressed = 0; -long lastDebounceTime = 0; -bool displayon = 1; -uint8_t loraMode = 0; // 0 init, 1 pickup, 2 RX, 3 TX -// ----------------------------------------------------------------------------- EEPROM / config -void saveConfig () { - for(size_t i = 0; i < sizeof(cfg); i++) { - char data = ((char *)&cfg)[i]; - EEPROM.write(i, data); - } - EEPROM.commit(); -} +msp_analog_t sys_curr_vbat; +char sys_curr_fc[5]; -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("IRP2").toCharArray(cfg.loraHeader,5); // protocol identifier - //cfg.loraAddress = 2; // local lora address - cfg.loraFrequency = 915E6; // 433E6, 868E6, 915E6 - cfg.loraBandwidth = 250000;// 250000 khz - cfg.loraCodingRate4 = 6; // Error correction rate 4/6 - cfg.loraSpreadingFactor = 8; // 7 is shortest time on air - 12 is longest - cfg.loraPower = 17; //17 PA OFF, 18-20 PA ON - cfg.loraPickupTime = 5; // in sec - cfg.intervalSend = 333; // in ms - cfg.intervalDisplay = 150; // in ms - cfg.intervalStatus = 1000; // in ms - cfg.uavTimeout = 8; // in sec - cfg.fcTimeout = 5; // 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) # TODO - cfg.debugOutput = false; - cfg.debugFakeWPs = false; - cfg.debugFakePlanes = false; - cfg.debugFakeMoving = false; - cfg.debugGpsLat = 50.100400 * 10000000; - cfg.debugGpsLon = 8.762835 * 10000000; - 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!"); - } -} -// ----------------------------------------------------------------------------- calc gps distance -#include -#include -#define earthRadiusKm 6371.0 +peer_t curr; // Our peer +peer_t incoming; // Incoming peer +peers_t peers[5]; // Other peers -double deg2rad(double deg) { - return (deg * M_PI / 180); -} +int sys_msp_ticker = 0; +int sys_num_peers = 0; -double rad2deg(double rad) { - return (rad * 180 / M_PI); -} +String sys_rssi = "0"; -/** - * Returns the distance between two points on the Earth. - * Direct translation from http://en.wikipedia.org/wiki/Haversine_formula - * @param lat1d Latitude of the first point in degrees - * @param lon1d Longitude of the first point in degrees - * @param lat2d Latitude of the second point in degrees - * @param lon2d Longitude of the second point in degrees - * @return The distance between the two points in kilometers - */ -double distanceEarth(double lat1d, double lon1d, double lat2d, double lon2d) { - double lat1r, lon1r, lat2r, lon2r, u, v; - lat1r = deg2rad(lat1d); - lon1r = deg2rad(lon1d); - lat2r = deg2rad(lat2d); - lon2r = deg2rad(lon2d); - u = sin((lat2r - lat1r)/2); - v = sin((lon2r - lon1r)/2); - return 2.0 * earthRadiusKm * asin(sqrt(u * u + cos(lat1r) * cos(lat2r) * v * v)); -} -// ----------------------------------------------------------------------------- String 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; - } - } - return found > index ? data.substring(strIndex[0], strIndex[1]) : ""; -} +uint8_t sys_pps = 0; +uint8_t sys_ppsc = 0; +bool io_button_state = 1; // Not needed ? +bool io_button_pressed = 0; +long io_button_released = 0; -// ----------------------------------------------------------------------------- CLI -SimpleCLI* cli; - -int serIn; // var that will hold the bytes-in read from the serialBuffer -char serInString[100]; // array that will hold the different bytes 100=100characters; - // -> you must state how long the array will be else it won't work. -int serInIndx = 0; // index of serInString[] in which to insert the next incoming byte -int serOutIndx = 0; // index of the outgoing serInString[] array; - -void readCli () { - int sb; - if(serialConsole[0]->available()) { - while (serialConsole[0]->available()){ - sb = serialConsole[0]->read(); - serInString[serInIndx] = sb; - serInIndx++; - serialConsole[0]->write(sb); - if (sb == '\n') { - cNum = 0; - cli->parse(serInString); - serInIndx = 0; - memset(serInString, 0, sizeof(serInString)); - serialConsole[0]->print("> "); - } - } - } -} -void cliLog (String log) { - //logger.append(log); - if (cfg.debugOutput) { - if (booted) { - Serial.println(); - Serial.print("LOG: "); - Serial.print(log); - } else { - Serial.println(log); - } - } -} -void cliStatus(int n) { - serialConsole[n]->println("================== Status =================="); - serialConsole[n]->print("FC: "); - serialConsole[n]->println(planeFC); - serialConsole[n]->print("Name: "); - serialConsole[n]->println(pd.planeName); - serialConsole[n]->print("Battery: "); - serialConsole[n]->print((float)fcanalog.vbat/10); - serialConsole[n]->println(" V"); - serialConsole[n]->print("Arm state: "); - serialConsole[n]->println(pd.state ? "ARMED" : "DISARMED"); - serialConsole[n]->print("GPS: "); - serialConsole[n]->println(String(pd.gps.numSat) + " Sats"); - serialConsole[n]->println("statusend"); -} -void cliHelp(int n) { - serialConsole[n]->println("================= Commands ================="); - serialConsole[n]->println("status - Show whats going on"); - serialConsole[n]->println("help - List all commands"); - serialConsole[n]->println("config - List all settings"); - serialConsole[n]->println("config loraFreq n - Set frequency in Hz (e.g. n = 433000000)"); - serialConsole[n]->println("config loraBandwidth n - Set bandwidth in Hz (e.g. n = 250000)"); - serialConsole[n]->println("config loraPower n - Set output power (e.g. n = 0 - 20)"); - serialConsole[n]->println("config uavtimeout n - Set UAV timeout in sec (e.g. n = 10)"); - serialConsole[n]->println("config fctimeout n - Set FC timeout in sec (e.g. n = 5)"); - serialConsole[n]->println("config radiointerval n - Set radio interval in ms (e.g. n = 500)"); - serialConsole[n]->println("config debuglat n - Set debug GPS lat (e.g. n = 50.1004900)"); - serialConsole[n]->println("config debuglon n - Set debug GPS lon (e.g. n = 8.7632280)"); - serialConsole[n]->println("reboot - Reset MCU and radio"); - serialConsole[n]->println("gpspos - Show last GPS position"); - //serialConsole[n]->println("fcpass - start FC passthru mode"); - serialConsole[n]->println("debug - Toggle debug output"); - serialConsole[n]->println("localfakeplanes - Send fake plane to FC"); - serialConsole[n]->println("lfp - Send fake plane to FC"); - serialConsole[n]->println("radiofakeplanes - Send fake plane via radio"); - serialConsole[n]->println("rfp - Send fake plane via radio"); - serialConsole[n]->println("movefakeplanes - Move fake plane"); - serialConsole[n]->println("mfp - Move fake plane"); -} -void cliConfig(int n) { - serialConsole[n]->println("=============== Configuration =============="); - serialConsole[n]->print("Lora frequency: "); - serialConsole[n]->print(cfg.loraFrequency); - serialConsole[n]->println(" Hz"); - serialConsole[n]->print("Lora bandwidth: "); - serialConsole[n]->print(cfg.loraBandwidth); - serialConsole[n]->println(" Hz"); - serialConsole[n]->print("Lora spreading factor: "); - serialConsole[n]->println(cfg.loraSpreadingFactor); - serialConsole[n]->print("Lora coding rate 4: "); - serialConsole[n]->println(cfg.loraCodingRate4); - serialConsole[n]->print("Lora power: "); - serialConsole[n]->println(cfg.loraPower); - serialConsole[n]->print("UAV timeout: "); - serialConsole[n]->print(cfg.uavTimeout); - serialConsole[n]->println(" sec"); - serialConsole[n]->print("FC timeout: "); - serialConsole[n]->print(cfg.fcTimeout); - serialConsole[n]->println(" sec"); - serialConsole[n]->print("Radio interval: "); - serialConsole[n]->print(cfg.intervalSend); - serialConsole[n]->println(" ms"); - serialConsole[n]->print("MSP RX pin: "); - serialConsole[n]->println(cfg.mspRX); - serialConsole[n]->print("MSP TX pin: "); - serialConsole[n]->println(cfg.mspTX); - serialConsole[n]->print("Debug output: "); - serialConsole[n]->println(cfg.debugOutput ? "ON" : "OFF"); - serialConsole[n]->print("Debug GPS lat: "); - serialConsole[n]->println((float) cfg.debugGpsLat / 10000000); - serialConsole[n]->print("Debug GPS lon: "); - serialConsole[n]->println((float) cfg.debugGpsLon / 10000000); - serialConsole[n]->print("Local fake planes: "); - serialConsole[n]->println(cfg.debugFakeWPs ? "ON" : "OFF"); - serialConsole[n]->print("Radio fake planes: "); - serialConsole[n]->println(cfg.debugFakePlanes ? "ON" : "OFF"); - serialConsole[n]->print("Move fake planes: "); - serialConsole[n]->println(cfg.debugFakeMoving ? "ON" : "OFF"); - serialConsole[n]->print("Firmware version: "); - serialConsole[n]->println(VERSION); - serialConsole[n]->println("cfgend"); -} -void cliShowLog(int n) { - //logger.flush(); -} -void cliDebug(int n) { - cfg.debugOutput = !cfg.debugOutput; - saveConfig(); - serialConsole[n]->println("CLI debugging: " + String(cfg.debugOutput)); -} -void cliLocalFake(int n) { - cfg.debugFakeWPs = !cfg.debugFakeWPs; - saveConfig(); - serialConsole[n]->println("Local fake planes: " + String(cfg.debugFakeWPs)); -} -void cliRadioFake(int n) { - cfg.debugFakePlanes = !cfg.debugFakePlanes; - saveConfig(); - serialConsole[n]->println("Radio fake planes: " + String(cfg.debugFakePlanes)); -} -void cliMoveFake(int n) { - cfg.debugFakeMoving = !cfg.debugFakeMoving; - saveConfig(); - serialConsole[n]->println("Move fake planes: " + String(cfg.debugFakeMoving)); -} -void cliReboot(int n) { - serialConsole[n]->println("Rebooting ..."); - serialConsole[n]->println(); - delay(1000); - ESP.restart(); -} -void cliGPSpos(int n) { - serialConsole[n]->print("Status: "); - serialConsole[n]->println(pd.gps.fixType ? String(pd.gps.numSat) + " Sats" : "no fix"); - serialConsole[n]->print("Position: "); - serialConsole[n]->print(pd.gps.lat/10000000); - serialConsole[n]->print(","); - serialConsole[n]->print(pd.gps.lon/10000000); - serialConsole[n]->print(","); - serialConsole[n]->println(pd.gps.alt); -} -void cliFCpass(int n) { - serialConsole[n]->println("=============== FC passthru ================"); +bool display_enabled = 1; +long display_updated = 0; + +uint8_t lora_mode = 0; // 0 init, 1 pickup, 2 RX, 3 TX +uint16_t lora_cycle_eff; + +// ----------------------------------------------------------------------------- EEPROM / config + +void config_init() { + + String("IRP2").toCharArray(cfg.lora_header,5); // protocol identifier + cfg.lora_frequency = 433E6; // 433E6, 868E6, 915E6 + cfg.lora_bandwidth = 250000;// KHz + cfg.lora_coding_rate = 5; // Error correction rate + cfg.lora_spreading_factor = 7; + cfg.lora_power = 17; //17 PA OFF, 18-20 PA ON + cfg.lora_pickup_delay = 5; // sec + cfg.lora_cycle = 500; // ms + cfg.lora_cycle_var = 50; // ms + cfg.display_cycle = 250; // ms + cfg.main_cycle = 1000; // ms + cfg.peer_timeout = 3; // 5 sec ************************ + cfg.msp_fc_timeout = 5; // sec + cfg.msp_tx_pin = 23; // pin for MSP serial TX + cfg.msp_rx_pin = 17; // pin for MSP serial RX } -void initCli () { - cli = new SimpleCLI(); - cli->onNotFound = [](String str) { - Serial.println("\"" + str + "\" not found"); - }; - cli->addCmd(new Command("status", [](Cmd* cmd) { cliStatus(cNum); } )); - cli->addCmd(new Command("help", [](Cmd* cmd) { cliHelp(cNum); } )); - cli->addCmd(new Command("debug", [](Cmd* cmd) { cliDebug(cNum); } )); - cli->addCmd(new Command("log", [](Cmd* cmd) { cliShowLog(cNum); } )); - cli->addCmd(new Command("localfakeplanes", [](Cmd* cmd) { cliLocalFake(cNum); } )); - cli->addCmd(new Command("radiofakeplanes", [](Cmd* cmd) { cliRadioFake(cNum); } )); - cli->addCmd(new Command("movefakeplanes", [](Cmd* cmd) { cliMoveFake(cNum); } )); - cli->addCmd(new Command("lfp", [](Cmd* cmd) { cliLocalFake(cNum); } )); - cli->addCmd(new Command("rfp", [](Cmd* cmd) { cliRadioFake(cNum); } )); - cli->addCmd(new Command("mfp", [](Cmd* cmd) { cliMoveFake(cNum); } )); - cli->addCmd(new Command("reboot", [](Cmd* cmd) { cliReboot(cNum); } )); - cli->addCmd(new Command("gpspos", [](Cmd* cmd) { cliGPSpos(cNum); } )); - - Command* config = new Command("config", [](Cmd* cmd) { - String arg1 = cmd->getValue(0); - String arg2 = cmd->getValue(1); - if (arg1 == "") cliConfig(cNum); - if (arg1 == "loraFreq") { - if ( arg2.toInt() == 433E6 || arg2.toInt() == 868E6 || arg2.toInt() == 915E6) { - cfg.loraFrequency = arg2.toInt(); - saveConfig(); - serialConsole[cNum]->println("Lora frequency changed!"); - } else { - serialConsole[cNum]->println("Lora frequency not correct: 433000000, 868000000, 915000000"); - } - } - if (arg1 == "loraBandwidth") { - if (arg2.toInt() == 250000 || arg2.toInt() == 62500 || arg2.toInt() == 7800) { - cfg.loraBandwidth = arg2.toInt(); - saveConfig(); - serialConsole[cNum]->println("Lora bandwidth changed!"); - } else { - serialConsole[cNum]->println("Lora bandwidth not correct: 250000, 62500, 7800"); - } - } - if (arg1 == "loraSpread") { - if (arg2.toInt() >= 7 && arg2.toInt() <= 12) { - cfg.loraSpreadingFactor = arg2.toInt(); - saveConfig(); - serialConsole[cNum]->println("Lora spreading factor changed!"); - } else { - serialConsole[cNum]->println("Lora spreading factor not correct: 7 - 12"); - } - } - if (arg1 == "loraPower") { - if (arg2.toInt() >= 0 && arg2.toInt() <= 20) { - cfg.loraPower = arg2.toInt(); - saveConfig(); - serialConsole[cNum]->println("Lora power factor changed!"); - } else { - serialConsole[cNum]->println("Lora power factor not correct: 0 - 20"); - } - } - if (arg1 == "fctimeout") { - if (arg2.toInt() >= 1 && arg2.toInt() <= 250) { - cfg.fcTimeout = arg2.toInt(); - saveConfig(); - serialConsole[cNum]->println("FC timout changed!"); - } else { - serialConsole[cNum]->println("FC timout not correct: 1 - 250"); - } - } - if (arg1 == "radiointerval") { - if (arg2.toInt() >= 50 && arg2.toInt() <= 1000) { - cfg.intervalSend = arg2.toInt(); - saveConfig(); - serialConsole[cNum]->println("Radio interval changed!"); - } else { - serialConsole[cNum]->println("Radio interval not correct: 50 - 1000"); - } - } - if (arg1 == "uavtimeout") { - if (arg2.toInt() >= 5 && arg2.toInt() <= 600) { - cfg.uavTimeout = arg2.toInt(); - saveConfig(); - serialConsole[cNum]->println("UAV timeout changed!"); - } else { - serialConsole[cNum]->println("UAV timeout not correct: 5 - 600"); - } - } - if (arg1 == "debuglat") { - if (1) { - serialConsole[cNum]->println(arg2); - float lat = arg2.toFloat() * 10000000; - cfg.debugGpsLat = (int32_t) lat; - saveConfig(); - serialConsole[cNum]->println("Debug GPS lat changed!"); - } else { - serialConsole[cNum]->println("Debug GPS lat not correct: 50.088251"); - } + +// ----------------------------------------------------------------------------- LoRa + +void lora_send(peer_t *outgoing) { + if (sys_msp_ticker < 255) { + sys_msp_ticker++; } - if (arg1 == "debuglon") { - if (1) { - float lon = arg2.toFloat() * 10000000; - cfg.debugGpsLon = (int32_t) lon; - saveConfig(); - serialConsole[cNum]->println("Debug GPS lon changed!"); - } else { - serialConsole[cNum]->println("Debug GPS lon not correct: 8.783871"); - } + else { + sys_msp_ticker = 0; } - }); - config->addArg(new AnonymOptArg("")); - config->addArg(new AnonymOptArg("")); + + + curr.tick = sys_msp_ticker; + curr.id = 1; + String("TEST9").toCharArray(curr.name, 5); - cli->addCmd(config); - //cli->parse("ping"); - //cli->parse("hello"); -} -// ----------------------------------------------------------------------------- Logger -void initLogger () { + while (!LoRa.beginPacket()) { } + LoRa.write((uint8_t*)outgoing, sizeof(curr)); + LoRa.endPacket(false); } -// ----------------------------------------------------------------------------- LoRa -void sendMessage(planeData *outgoing) { - if (loraSeqNum < 255) loraSeqNum++; - else loraSeqNum = 0; - pd.seqNum = loraSeqNum; - while (!LoRa.beginPacket()) { } - LoRa.write((uint8_t*)outgoing, sizeof(fakepd)); - LoRa.endPacket(false); -} void onReceive(int packetSize) { - if (packetSize == 0) return; - LoRa.readBytes((uint8_t *)&loraMsg, packetSize); - //cliLog(loraMsg.header); - //cliLog(cfg.loraHeader); - if (String(loraMsg.header) == String(cfg.loraHeader)) { // new plane data - //cliLog(cfg.loraHeader); - rssi = String(LoRa.packetRssi()); - ppsc++; - loraRX = 1; - pdIn = loraMsg; - cliLog("New air packet"); - uint8_t id = loraMsg.id - 1; - pds[id].id = loraMsg.id; - pds[id].pd = loraMsg; - pds[id].rssi = LoRa.packetRssi(); - pds[id].pd.state = 1; - pds[id].lastUpdate = millis(); - cliLog("UAV DB update POI #" + String(loraMsg.id)); - } -} -int moving = 0; -void sendFakePlanes () { - if (loraSeqNum < 255) loraSeqNum++; - else loraSeqNum = 0; - pd.seqNum = loraSeqNum; - if (cfg.debugFakeMoving && moving > 100) { - moving = 0; - } else { - if (!cfg.debugFakeMoving) moving = 0; - } - cliLog("Sending fake UAVs via radio ..."); - String("IRP2").toCharArray(fakepd.header,5); - //fakepd.loraAddress = (char)5; - String("Testplane #1").toCharArray(fakepd.planeName,20); - if (loraSeqNum < 255) loraSeqNum++; - else loraSeqNum = 0; - fakepd.seqNum = loraSeqNum; - fakepd.state= 1; - fakepd.id = pd.id; - fakepd.gps.alt = 300; - fakepd.gps.groundSpeed = 450; - fakepd.gps.lat = cfg.debugGpsLat; // + (250 * moving); - fakepd.gps.lon = cfg.debugGpsLon + (250 * moving); - sendMessage(&fakepd); - cliLog("Fake UAVs sent."); - moving++; + if (packetSize == 0) return; + LoRa.readBytes((uint8_t *)&incoming, packetSize); + + if (String(incoming.header) == String(cfg.lora_header)) { // New data from a peer + + sys_rssi = String(LoRa.packetRssi()); + sys_ppsc++; + + +// uint8_t id = incoming.id - 1; + uint8_t id = incoming.id; + peers[id].id = incoming.id; + peers[id].updated = millis(); + peers[id].rssi = LoRa.packetRssi(); + peers[id].peer = incoming; + peers[id].peer.state = 1; + + } } -void initLora() { - display.drawString (0, 16, "LORA"); - display.display(); - cliLog("Starting radio ..."); - //pd.loraAddress = cfg.loraAddress; - String(cfg.loraHeader).toCharArray(pd.header,5); - SPI.begin(5, 19, 27, 18); - LoRa.setPins(SS,RST,DI0); - if (!LoRa.begin(cfg.loraFrequency)) { - cliLog("Radio start failed!"); - display.drawString (94, 8, "FAIL"); - while (1); - } - LoRa.sleep(); - LoRa.setSignalBandwidth(cfg.loraBandwidth); - LoRa.setCodingRate4(cfg.loraCodingRate4); - LoRa.setSpreadingFactor(cfg.loraSpreadingFactor); - LoRa.setTxPower(cfg.loraPower,1); - LoRa.setOCP(200); - LoRa.idle(); - LoRa.onReceive(onReceive); - LoRa.enableCrc(); - pd.id = 0; - loraMode = LA_INIT; - LoRa.receive(); - cliLog("Radio started."); - display.drawString (100, 16, "OK"); - display.display(); + +void lora_init() { + display.drawString (0, 16, "LORA"); + display.display(); + + String(cfg.lora_header).toCharArray(curr.header,5); + SPI.begin(5, 19, 27, 18); + LoRa.setPins(SS, RST, DI0); + + if (!LoRa.begin(cfg.lora_frequency)) { + display.drawString (94, 8, "FAIL"); + while (1); + } + + LoRa.sleep(); + LoRa.setSignalBandwidth(cfg.lora_bandwidth); + LoRa.setCodingRate4(cfg.lora_coding_rate); + LoRa.setSpreadingFactor(cfg.lora_spreading_factor); + LoRa.setTxPower(cfg.lora_power, 1); + LoRa.setOCP(200); + LoRa.idle(); + LoRa.onReceive(onReceive); + LoRa.enableCrc(); + LoRa.receive(); } + // ----------------------------------------------------------------------------- Display -void displayArmed () { - display.clear(); - display.setFont (ArialMT_Plain_24); - display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (20,20, "Armed"); - display.display(); - delay(250); - display.clear(); - display.display(); - displayon = 0; + +void display_init() { + pinMode(16, OUTPUT); + pinMode(2, OUTPUT); + digitalWrite(16, LOW); + delay(50); + digitalWrite(16, HIGH); + display.init(); + display.flipScreenVertically(); + display.setFont(ArialMT_Plain_10); + display.setTextAlignment(TEXT_ALIGN_LEFT); } -void displayDisarmed () { - display.clear(); - display.setFont (ArialMT_Plain_24); - display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (20,20, "Disarmed"); - display.display(); - delay(250); - displayon = 1; + +void display_disable() { + display.clear(); + display.setFont (ArialMT_Plain_24); + display.setTextAlignment (TEXT_ALIGN_LEFT); + display.drawString (20, 20, "Armed"); + display.display(); + delay(250); + display.clear(); + display.display(); + display_enabled = 0; } -void drawDisplay () { - display.clear(); - if (displayPage == 0) { + +void display_enable() { + display.clear(); display.setFont (ArialMT_Plain_24); - display.setTextAlignment (TEXT_ALIGN_RIGHT); - display.drawString (30,5, String(pd.gps.numSat)); - display.drawString (30,30, String(numPlanes)); - display.drawString (114,5, String((float)fcanalog.vbat/10)); - display.setFont (ArialMT_Plain_10); - display.drawString (110,30, String(pps)); - display.drawString (110,42, rssi); display.setTextAlignment (TEXT_ALIGN_LEFT); - //if (pd.gps.fixType == 0) display.drawString (33,7, "NF"); - if (pd.gps.fixType == 1) display.drawString (33,7, "2D"); - if (pd.gps.fixType == 2) display.drawString (33,7, "3D"); - display.drawString (33,16, "SAT"); - display.drawString (33,34, "ID"); - display.drawString (49,34, String(pd.id)); - display.drawString (33,42, "UAV"); - display.drawString (116,16, "V"); - display.drawString (112,30, "pps"); - display.drawString (112,42, "dB"); - if (String(planeFC) == String("No FC")) { - if (cfg.debugOutput) { - if (cfg.debugFakeWPs) display.drawString (0,54, "LFP"); - if (cfg.debugFakePlanes) display.drawString (30,54, "RFP"); - if (cfg.debugFakeMoving) display.drawString (60,54, "MFP"); - } - } else { - //if (bitRead(fcstatusex.armingFlags,17) == 0) display.drawString (0,54, "RC LINK LOST"); - //else - // TODO - display.drawString (0,54, pd.planeName); - //if (fcstatusex.armingFlags != 0) display.drawXbm(61, 54, 8, 8, warnSymbol); + display.drawString (20,20, "Disarmed"); + display.display(); + delay(250); + display_enabled = 1; +} + +void display_draw() { + display.clear(); + if (sys_display_page == 0) { + display.setFont (ArialMT_Plain_24); + display.setTextAlignment (TEXT_ALIGN_RIGHT); + display.drawString (30,5, String(curr.gps.numSat)); + display.drawString (30,30, String(sys_num_peers)); + display.drawString (114,5, String((float)sys_curr_vbat.vbat/10)); + display.setFont (ArialMT_Plain_10); + display.drawString (110,30, String(sys_pps)); + display.drawString (110,42, sys_rssi); + +display.drawString (60,54, String(sys_msp_ticker)); + + display.setTextAlignment (TEXT_ALIGN_LEFT); + if (curr.gps.fixType == 1) display.drawString (33,7, "2D"); + if (curr.gps.fixType == 2) display.drawString (33,7, "3D"); + display.drawString (33,16, "SAT"); + display.drawString (33,34, "ID"); + display.drawString (49,34, String(curr.id)); + display.drawString (33,42, "UAV"); + display.drawString (116,16, "V"); + display.drawString (112,30, "Hz"); + display.drawString (112,42, "dB"); + + if (String(sys_curr_fc) != String("NO FC")) { + display.drawString (0,54, sys_curr_fc); + } +// display.drawString (84, 54, display_lora_tx ? "TX" : " "); +// display.drawString (106, 54, display_lora_rx ? "RX" : " "); } - display.drawString (84,54, "TX"); - display.drawString (106,54, "RX"); - display.drawXbm(98, 55, 8, 8, loraTX ? activeSymbol : inactiveSymbol); - display.drawXbm(120, 55, 8, 8, loraRX ? activeSymbol : inactiveSymbol); - } - if (displayPage == 1) { - if (numPlanes == 0) display.drawString (0,0, "no UAVs detected ..."); - else { - display.setFont (ArialMT_Plain_10); - for (size_t i = 0; i <=2 ; i++) { - if (pds[i].id != 0) { - display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (0,i*16, pds[i].pd.planeName); - display.drawString (0,8+i*16, "RSSI " + String(pds[i].rssi)); - display.setTextAlignment (TEXT_ALIGN_RIGHT); - display.drawString (128,i*16, String((float)pds[i].pd.gps.lat/10000000,6)); - display.drawString (128,8+i*16, String((float)pds[i].pd.gps.lon/10000000,6)); + + else if (sys_display_page == 1) { + if (sys_num_peers == 0) { + display.drawString (0, 0, "No UAV detected"); + } + else { + display.setFont (ArialMT_Plain_10); + for (size_t i = 0; i <=2 ; i++) { + if (peers[i].id != 0) { + display.setTextAlignment (TEXT_ALIGN_LEFT); + display.drawString (0, i*16, peers[i].peer.name); + display.drawString (0, 8 + i * 16, "RSSI " + String(peers[i].rssi)); + display.setTextAlignment (TEXT_ALIGN_RIGHT); + display.drawString (128, i * 16, String((float)peers[i].peer.gps.lat/10000000, 6)); + display.drawString (128, 8 + i * 16, String((float)peers[i].peer.gps.lon/10000000, 6)); + } + } } - } } - } - if (displayPage == 2) { - display.setFont (ArialMT_Plain_10); - for (size_t i = 0; i <=1 ; i++) { - if (pds[i].id != 0) { - display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (0,i*16, pds[i+3].pd.planeName); - display.drawString (0,8+i*16, "RSSI " + String(pds[i].rssi)); - display.setTextAlignment (TEXT_ALIGN_RIGHT); - display.drawString (65,i*16, String((float)pds[i+3].pd.gps.lat/10000000,6)); - display.drawString (65,8+i*16, String((float)pds[i+3].pd.gps.lon/10000000,6)); - } + + else if (sys_display_page == 2) { + display.setFont (ArialMT_Plain_10); + for (size_t i = 0; i <=1 ; i++) { + if (peers[i].id != 0) { + display.setTextAlignment (TEXT_ALIGN_LEFT); + display.drawString (0, i*16, peers[i+3].peer.name); + display.drawString (0, 8 + i * 16, "RSSI " + String(peers[i].rssi)); + display.setTextAlignment (TEXT_ALIGN_RIGHT); + display.drawString (65, i * 16, String((float)peers[i+3].peer.gps.lat/10000000, 6)); + display.drawString (65, 8 + i * 16, String((float)peers[i+3].peer.gps.lon/10000000, 6)); + } + } } - } - display.display(); + display.display(); } -void initDisplay () { - cliLog("Starting display ..."); - pinMode (16, OUTPUT); - pinMode (2, OUTPUT); - digitalWrite (16, LOW); // set GPIO16 low to reset OLED - delay (50); - digitalWrite (16, HIGH); // while OLED is running, GPIO16 must go high - display.init (); - display.flipScreenVertically (); - display.setFont (ArialMT_Plain_10); - display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (0, 0, "DISPLAY"); - display.drawString (100, 0, "OK"); - display.drawString (0, 8, "FIRMWARE"); - display.drawString (100, 8, VERSION); - display.display(); - cliLog("Display started!"); -} + + + // ----------------------------------------------------------------------------- MSP and FC -void getPlanetArmed () { - uint32_t planeModes; - msp.getActiveModes(&planeModes); - pd.state = bitRead(planeModes,0); - cliLog("FC: Arm state " + String(pd.state)); -} -void getPlaneGPS () { - if (msp.request(MSP_RAW_GPS, &pd.gps, sizeof(pd.gps))) { - cliLog("FC: GPS " + String(pd.gps.fixType)); - } -} -void getPlaneBat () { - if (msp.request(MSP_ANALOG, &fcanalog, sizeof(fcanalog))) { - cliLog("FC: Bat " + String(fcanalog.vbat)); - } + +void msp_set_curr_state() { + uint32_t planeModes; + msp.getActiveModes(&planeModes); + curr.state = bitRead(planeModes, 0); } -void getPlaneStatusEx () { - if (msp.request(MSP_STATUS_EX, &fcstatusex, sizeof(fcstatusex))) { - cliLog("FC: Status EX " + String(bitRead(fcstatusex.armingFlags,18))); - } + +void msp_set_curr_gps() { + msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); } -void getPlaneData () { - for (size_t i = 0; i < 5; i++) { - pd.planeName[i] = (char) random(65,90); - } - if (msp.request(MSP_NAME, &pd.planeName, sizeof(pd.planeName))) { - cliLog("FC: UAV name " + String(pd.planeName)); - } - String("No FC").toCharArray(planeFC,20); - if (msp.request(MSP_FC_VARIANT, &planeFC, sizeof(planeFC))) { - cliLog("FC: Firmware " + String(planeFC)); - } + +void msp_set_curr_vbat() { + msp.request(MSP_ANALOG, &sys_curr_vbat, sizeof(sys_curr_vbat)); } -void planeSetWP () { - msp_radar_pos_t radarPos; - for (size_t i = 0; i <= 4; i++) { - if (pds[i].id != 0) { -// wp.waypointNumber = pds[i].waypointNumber; -// wp.action = MSP_NAV_STATUS_WAYPOINT_ACTION_WAYPOINT; - radarPos.id = i; - radarPos.state = pds[i].pd.state; - radarPos.lat = pds[i].pd.gps.lat; - radarPos.lon = pds[i].pd.gps.lon; - radarPos.alt = pds[i].pd.gps.alt; - radarPos.speed = pds[i].pd.gps.groundSpeed; - radarPos.heading = pds[i].pd.gps.groundCourse / 10; - if (millis() - pds[i].lastUpdate < cfg.intervalSend+100) radarPos.lq = constrain ( (130 + pds[i].rssi ) , 1 , 100); - else radarPos.lq = 0; - msp.command(MSP_SET_RADAR_POS, &radarPos, sizeof(radarPos)); - cliLog("Sent to FC - POI #" + String(i)); + + +void msp_set_curr_name() { + char buff[16]; + if (!msp.request(MSP_NAME, &buff, sizeof(curr.name))) { + for (size_t i = 0; i < 5; i++) { + curr.name[i] = (char) random(65, 90); + } + } else { + for (size_t i = 0; i < 5; i++) { + curr.name[i] = buff[i]; // ------ + } } - } } -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 msp_set_curr_fc() { + if (!msp.request(MSP_FC_VARIANT, &sys_curr_fc, sizeof(sys_curr_fc))) { + String("NoFC ").toCharArray(sys_curr_fc, 5); + } } -void planeFakeWP () { - msp_set_wp_t wp; - if (cfg.debugFakeMoving && moving > 100) { - moving = 0; - } - if (!cfg.debugFakeMoving) { - moving = 0; - } - if (pd.gps.fixType > 0) { - wp.waypointNumber = 1; - wp.action = MSP_NAV_STATUS_WAYPOINT_ACTION_WAYPOINT; - wp.lat = homepos.lat-100 + (moving * 20); - wp.lon = homepos.lon; - wp.alt = homepos.alt + 300; - wp.p1 = 1000; - wp.p2 = 0; - wp.p3 = 1; - wp.flag = 0xa5; - msp.command(MSP_SET_WP, &wp, sizeof(wp)); - cliLog("Fake POIs sent to FC."); - } else { - cliLog("Fake POIs waiting for GPS fix."); + +void msp_send_peers() { + msp_radar_pos_t radarPos; + for (size_t i = 0; i <= 4; i++) { + if (peers[i].id != 0) { + radarPos.id = i; + radarPos.state = peers[i].peer.state; + radarPos.lat = peers[i].peer.gps.lat; + radarPos.lon = peers[i].peer.gps.lon; + radarPos.alt = peers[i].peer.gps.alt; + radarPos.speed = peers[i].peer.gps.groundSpeed; + radarPos.heading = peers[i].peer.gps.groundCourse / 10; + msp.command(MSP_SET_RADAR_POS, &radarPos, sizeof(radarPos)); + } } } -void initMSP () { - cliLog("Starting MSP ..."); - display.drawString (0, 24, "MSP "); - display.display(); - delay(200); - Serial1.begin(57600, SERIAL_8N1, cfg.mspRX , cfg.mspTX); - msp.begin(Serial1); - cliLog("MSP started!"); - display.drawString (100, 24, "OK"); - display.display(); - display.drawString (0, 32, "FC "); - display.display(); - cliLog("Waiting for FC to start ..."); - delay(cfg.fcTimeout*1000); - getPlaneData(); - getPlaneGPS(); - cliLog("FC detected: " + String(planeFC)); - display.drawString (100, 32, planeFC); - display.display(); +void msp_init() { + + display.drawString (0, 24, "MSP "); + display.display(); + delay(100); + Serial1.begin(57600, SERIAL_8N1, cfg.msp_rx_pin , cfg.msp_tx_pin); + msp.begin(Serial1); + + display.drawString (100, 24, "OK"); + display.display(); + display.drawString (0, 32, "FC "); + display.display(); + + delay(cfg.msp_fc_timeout * 1000); + msp_set_curr_name(); + msp_set_curr_fc(); + msp_set_curr_gps(); + + display.drawString (100, 32, sys_curr_fc); + display.display(); } + // ----------------------------------------------------------------------------- main init const byte interruptPin = 0; @@ -760,153 +337,171 @@ int numberOfInterrupts = 0; portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED; void IRAM_ATTR handleInterrupt() { - portENTER_CRITICAL_ISR(&mux); - if (buttonPressed == 0) { - buttonPressed = 1; - if (displayPage >= 1) displayPage = 0; - else displayPage++; - //drawDisplay(); - lastDebounceTime = millis(); - } - portEXIT_CRITICAL_ISR(&mux); + portENTER_CRITICAL_ISR(&mux); + + if (io_button_pressed == 0) { + io_button_pressed = 1; + + if (sys_display_page >= 1) { + sys_display_page = 0; + } + else { + sys_display_page++; + } + + io_button_released = millis(); + } + portEXIT_CRITICAL_ISR(&mux); } void setup() { - Serial.begin(115200); - serialConsole[0] = &Serial; - initLogger(); - initCli(); - initConfig(); - initDisplay(); - initLora(); - delay(1500); - initMSP(); - delay(1000); - pinMode(interruptPin, INPUT); - buttonPressed = 0; - attachInterrupt(digitalPinToInterrupt(interruptPin), handleInterrupt, RISING); - for (size_t i = 0; i <= 4; i++) { - pds[i].id = 0; - } - booted = 1; - serialConsole[0]->print("> "); +// Serial.begin(115200); +// serialConsole[0] = &Serial; + + config_init(); + + display_init(); + display.drawString(0, 8, "FIRMWARE"); + display.drawString(100, 8, VERSION); + display.display(); + + lora_init(); + + display.drawString (100, 16, "OK"); + display.display(); + + delay(500); + + msp_init(); + delay(500); + + pinMode(interruptPin, INPUT); + io_button_pressed = 0; + attachInterrupt(digitalPinToInterrupt(interruptPin), handleInterrupt, RISING); + + for (size_t i = 0; i <= 4; i++) { + peers[i].id = 0; + } + + curr.id = 0; + lora_mode = LORA_RX; + lora_cycle_eff = cfg.lora_cycle; + + lora_last_tx = millis(); + main_updated = millis(); + display_updated = millis(); + } -// ----------------------------------------------------------------------------- main loop +// ----------------------------------------------------------------------------- MAIN LOOP + void loop() { - if ( (millis() - lastDebounceTime) > 150 && buttonPressed == 1) buttonPressed = 0; - - if (loraMode == LA_INIT && pd.id == 0) { - loraMode = LA_PICKUP; - pickupTime = millis(); - } - - if (loraMode == LA_PICKUP && millis() - pickupTime > cfg.loraPickupTime * 1000) { - numPlanes = 0; - for (size_t i = 0; i < sizeof(pds); i++) { - if (pds[i].id == 0 && pd.id == 0) { - pd.id = i+1; - loraMode = LA_RX; - } - if (pds[i].id != 0) numPlanes++; - } - if (pds[0].id == 1) sendLastTime = pds[0].lastUpdate; - } - if (loraMode == LA_RX) { - if (pd.id == 1 && (millis() - sendLastTime) > cfg.intervalSend) { - loraMode = LA_TX; - cliLog("Master TX" + String(millis() - sendLastTime)); - sendLastTime = millis(); +// ---------------------- IO BUTTON - } - if (pd.id > 1 && currentUpdateTime != pds[0].lastUpdate && millis() - pds[0].lastUpdate > cfg.intervalSend / 5 * pd.id) { - loraMode = LA_TX; - currentUpdateTime = pds[0].lastUpdate; - cliLog(String(pd.id) + " TX in time " + String(millis() - sendLastTime)); - sendLastTime = millis(); - } else if (pd.id > 1 && (millis() - sendLastTime) > cfg.intervalSend && currentUpdateTime == pds[0].lastUpdate) { - loraMode = LA_TX; - cliLog(String(pd.id) + " TX no time " + String(millis() - sendLastTime)); - sendLastTime = millis(); + if ((millis() - io_button_released) > 150 && io_button_pressed == 1) { + io_button_pressed = 0; } - } - - if (displayon && millis() - displayLastTime > cfg.intervalDisplay) { - drawDisplay(); - readCli(); - loraTX = 0; - loraRX = 0; - displayLastTime = millis(); - if (pd.state == 1) displayArmed(); - } - - if (millis() - pdLastTime > cfg.intervalStatus) { - numPlanes = 0; - pps = ppsc; - ppsc = 0; - for (size_t i = 0; i <= 4; i++) { - if (pds[i].id != 0) numPlanes++; - //if (pd.gps.fixType != 0) pds[i].distance = distanceEarth(pd.gps.lat/10000000, pd.gps.lon/10000000, pds[i].pd.gps.lat/10000000, pds[i].pd.gps.lon/10000000); - if (pds[i].id != 0 && millis() - pds[i].lastUpdate > cfg.uavTimeout*1000 ) { // plane timeout - pds[i].pd.state = 2; - planeSetWP(); - pds[i].id = 0; - rssi = "0"; - String("").toCharArray(pds[i].pd.planeName,20); - cliLog("UAV DB delete POI #" + String(i+1)); - } +// ---------------------- LORA INIT +/* + if (lora_mode == LORA_INIT && curr.id == 0) { + lora_mode = LORA_PICKUP; + sys_pickup_updated = millis(); } - //getPlaneStatusEx(); - if (String(planeFC) == "INAV" ) { - getPlanetArmed(); - getPlaneBat(); - if (pd.state == 0) { - getPlaneGPS(); - if (displayon == 0) displayDisarmed(); - if (pd.gps.fixType != 0) { - homepos = pd.gps; +*/ +// ---------------------- LORA PICKUP +/* + if ((lora_mode == LORA_PICKUP) && (millis() - sys_pickup_updated > cfg.lora_pickup_delay * 1000)) { + sys_num_peers = 0; + for (int i = 0; i < sizeof(peers); i++) { + if (peers[i].id == 0 && curr.id == 0) { + curr.id = i + 1; + lora_mode = LORA_RX; + } + if (peers[i].id != 0) { + sys_num_peers++; + } + } + if (peers[0].id == 1) { + lora_last_tx = peers[0].updated; } - } - } - pdLastTime = millis(); - } - - if (loraMode == LA_TX) { - if (pd.state != 1) { - if (pd.gps.fixType != 0) { - homepos = pd.gps; - sendMessage(&pd); - loraTX = 1; - } else { - pd.state = 2; - sendMessage(&pd); - loraTX = 1; - } - LoRa.sleep(); - LoRa.receive(); } +*/ +// ---------------------- LORA RX + + + if ((millis() - lora_last_tx) > lora_cycle_eff) { + lora_mode = LORA_TX; + lora_cycle_eff = cfg.lora_cycle - random(0, cfg.lora_cycle_var); + lora_last_tx = millis(); + } + + +// ---------------------- LORA TX + + if (lora_mode == LORA_TX) { + + + // msp_set_curr_gps(); + + // if (curr.gps.fixType > 0) { + lora_send(&curr); + + // } + + if (String(sys_curr_fc) == "INAV" ) { + msp_send_peers(); + } - if (pd.state == 1) { - getPlaneGPS(); - loraTX = 1; - if (pd.gps.fixType != 0) { - sendMessage(&pd); LoRa.sleep(); LoRa.receive(); - } + + lora_mode = LORA_RX; } - //if (pd.armState) planeSetWP(); - if (String(planeFC) == "INAV" ) planeSetWP(); - if (cfg.debugFakeWPs) planeFakeWP(); - if (cfg.debugFakePlanes) { - sendFakePlanes(); - LoRa.sleep(); - LoRa.receive(); - loraTX = 1; + + + if ((display_enabled && ((millis() - display_updated)) > cfg.display_cycle)) { + + display_draw(); + display_updated = millis(); } - //if (pd.id == 1) sendLastTime = millis(); - loraMode = LA_RX; - } + + + if ((millis() - main_updated) > cfg.main_cycle) { + sys_num_peers = 0; + sys_pps = sys_ppsc; + sys_ppsc = 0; + + for (size_t i = 0; i <= 4; i++) { + if (peers[i].id != 0) { + sys_num_peers++; + } + + if (peers[i].id != 0 && ((millis() - peers[i].updated) > cfg.peer_timeout * 1000)) { // Peer timeout + peers[i].peer.state = 0; + // msp_send_peers(); + peers[i].id = 0; + sys_rssi = "0"; + String("").toCharArray(peers[i].peer.name, 5); + } + } + + if (String(sys_curr_fc) == "INAV" ) { + msp_set_curr_state(); + msp_set_curr_vbat(); + } + + if ((curr.state == 0) && (display_enabled == 0)) { + display_enable(); + } + + else if (curr.state == 1 && (display_enabled == 1)) { // Aircraft is armed = Turning off the OLED + display_disable(); + } + + main_updated = millis(); + } + + } diff --git a/src/main.h b/src/main.h index 6effbb6..c154582 100644 --- a/src/main.h +++ b/src/main.h @@ -1,79 +1,41 @@ -// lora modes -#define LA_INIT 0 -#define LA_PICKUP 1 -#define LA_RX 2 -#define LA_TX 3 + +#define LORA_INIT 0 +#define LORA_PICKUP 1 +#define LORA_RX 2 +#define LORA_TX 3 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? - uint8_t loraPower; // 0-20 - uint8_t loraPickupTime; // 5 sec - uint16_t intervalSend; // in ms + random - uint16_t intervalDisplay; // in ms - uint16_t intervalStatus; // in ms - uint16_t uavTimeout; // in sec - uint8_t fcTimeout; // 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; - int32_t debugGpsLat; // decimal degrees lat * 10000000 - int32_t debugGpsLon; // decimal degrees lon * 10000000 + char lora_header[7]; + uint32_t lora_frequency; + uint32_t lora_bandwidth; + uint8_t lora_coding_rate; + uint8_t lora_spreading_factor; + uint8_t lora_power; + uint8_t lora_pickup_delay; + uint16_t lora_cycle; + uint8_t lora_cycle_var; + uint16_t display_cycle; + uint16_t main_cycle; + uint16_t peer_timeout; + uint8_t msp_fc_timeout; + uint8_t msp_tx_pin; + uint8_t msp_rx_pin; }; -struct planeData { +struct peer_t { char header[5]; uint8_t id; - uint8_t seqNum; - char planeName[20]; + uint8_t tick; + char name[5]; uint8_t state; msp_raw_gps_t gps; }; -struct planesData { + +struct peers_t { uint8_t id; - long lastUpdate; - double distance; + long updated; double rssi; - planeData pd; -}; -const uint8_t activeSymbol[] PROGMEM = { - B00000000, - B00000000, - B00011000, - B00100100, - B01000010, - B01000010, - B00100100, - B00011000 + peer_t peer; }; -const uint8_t inactiveSymbol[] PROGMEM = { - B00000000, - B00000000, - B00000000, - B00000000, - B00011000, - B00011000, - B00000000, - B00000000 -}; -const uint8_t warnSymbol[] PROGMEM = { - B00000000, - B00000000, - B00010000, - B00101000, - B00101000, - B01111100, - B01101100, - B11111110 -}; From c1ae1e5272fd3077c28b24ca95e7a8485a53f51b Mon Sep 17 00:00:00 2001 From: Olivier C Date: Tue, 16 Apr 2019 18:45:09 +0200 Subject: [PATCH 02/17] Lot of new things --- src/main.cpp | 398 ++++++++++++++++++++++++++++++++++----------------- src/main.h | 47 ++++-- 2 files changed, 299 insertions(+), 146 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 29f94b7..b5c616a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -14,6 +14,7 @@ #define DI0 26 // GPIO26 - SX1278's IRQ (interrupt request) #define VERSION "Z1" +#define LORA_MAXPEERS 5 // ----------------------------------------------------------------------------- global vars config cfg; @@ -34,11 +35,12 @@ msp_analog_t sys_curr_vbat; char sys_curr_fc[5]; peer_t curr; // Our peer -peer_t incoming; // Incoming peer -peers_t peers[5]; // Other peers +peer_t peers[5]; // Other peers +peer_air_t incoming; // Received +peer_air_t j; // Sent -int sys_msp_ticker = 0; -int sys_num_peers = 0; +int sys_tx_ticker = 0; +int sys_num_peers = -1; String sys_rssi = "0"; @@ -52,26 +54,34 @@ long io_button_released = 0; bool display_enabled = 1; long display_updated = 0; -uint8_t lora_mode = 0; // 0 init, 1 pickup, 2 RX, 3 TX -uint16_t lora_cycle_eff; +long cycle_scan_begin; +uint8_t scan_last_peers = -1; + +int lora_mode = LORA_INIT; + +int last_received_id = 0; +char last_received_name[5]; // ----------------------------------------------------------------------------- EEPROM / config void config_init() { - String("IRP2").toCharArray(cfg.lora_header,5); // protocol identifier + String("R01").toCharArray(cfg.lora_header, 3); // protocol identifier cfg.lora_frequency = 433E6; // 433E6, 868E6, 915E6 cfg.lora_bandwidth = 250000;// KHz cfg.lora_coding_rate = 5; // Error correction rate - cfg.lora_spreading_factor = 7; - cfg.lora_power = 17; //17 PA OFF, 18-20 PA ON - cfg.lora_pickup_delay = 5; // sec - cfg.lora_cycle = 500; // ms - cfg.lora_cycle_var = 50; // ms - cfg.display_cycle = 250; // ms - cfg.main_cycle = 1000; // ms - cfg.peer_timeout = 3; // 5 sec ************************ - cfg.msp_fc_timeout = 5; // sec + cfg.lora_spreading_factor = 8; + cfg.lora_power = 20; //17 PA OFF, 18-20 PA ON + + cfg.cycle_lora = 480; // ms + cfg.cycle_lora_slotspacing = 96; // ms + cfg.cycle_scan = 4000; // ms + cfg.cycle_display = 150; // ms + cfg.cycle_main = 1000; // ms + + cfg.peer_timeout = 3; // sec + cfg.msp_fc_timeout = 2; // sec + cfg.msp_tx_pin = 23; // pin for MSP serial TX cfg.msp_rx_pin = 17; // pin for MSP serial RX } @@ -79,46 +89,86 @@ void config_init() { // ----------------------------------------------------------------------------- LoRa -void lora_send(peer_t *outgoing) { - if (sys_msp_ticker < 255) { - sys_msp_ticker++; - } - else { - sys_msp_ticker = 0; - } - - - curr.tick = sys_msp_ticker; - curr.id = 1; - String("TEST9").toCharArray(curr.name, 5); +void lora_send(peer_air_t *outgoing) { while (!LoRa.beginPacket()) { } - LoRa.write((uint8_t*)outgoing, sizeof(curr)); + LoRa.write((uint8_t*)outgoing, sizeof(outgoing)); LoRa.endPacket(false); } + + void onReceive(int packetSize) { if (packetSize == 0) return; LoRa.readBytes((uint8_t *)&incoming, packetSize); - if (String(incoming.header) == String(cfg.lora_header)) { // New data from a peer + // if (String(incoming.header) == String(cfg.lora_header)) { // New data from a peer + + sys_rssi = LoRa.packetRssi(); + sys_ppsc++; +/* + if (String(incoming.name) == peers[incoming.id].name) { // Known peer + peers[incoming.id].state = 1; + } + else { // Something wrong, id slot / name mismatch + peers[incoming.id].state = 1; // TODO, for now overwrite + } +*/ + uint8_t id = incoming.id - 1; + peers[id].id = incoming.id; + last_received_id = incoming.id; + + peers[id].name[0] = incoming.name[0]; + peers[id].name[1] = incoming.name[1]; + peers[id].name[2] = incoming.name[2]; + peers[id].name[3] = incoming.name[3]; + peers[id].name[4] = incoming.name[4]; +/* + peers[id].header[0] = incoming.header[0]; + peers[id].header[1] = incoming.header[1]; + peers[id].header[2] = incoming.header[2]; +*/ + peers[id].tick = incoming.tick; + peers[id].updated = millis(); + // peers[id].rssi = sys_rssi; + peers[id].gps.lat = incoming.lat; + peers[id].gps.lon = incoming.lon; + peers[id].gps.alt = incoming.alt; + + peers[id].gps.lat = 0; + peers[id].gps.lon = 0; + peers[id].gps.alt = 0; + +// peers[id].gps.groundSpeed = incoming.speed; + peers[id].gps.groundCourse = incoming.heading; + // } +} + + +/* +void onReceive(int packetSize) { + if (packetSize == 0) return; + LoRa.readBytes((uint8_t *)&incoming, packetSize); + + //if (String(incoming.header) == String(cfg.lora_header)) { // new plane data sys_rssi = String(LoRa.packetRssi()); sys_ppsc++; -// uint8_t id = incoming.id - 1; - uint8_t id = incoming.id; + uint8_t id = incoming.id - 1; peers[id].id = incoming.id; - peers[id].updated = millis(); - peers[id].rssi = LoRa.packetRssi(); - peers[id].peer = incoming; - peers[id].peer.state = 1; - } + peers[id].rssi = LoRa.packetRssi(); + peers[id].state = 1; + peers[id].updated = millis(); +//} } +*/ + + void lora_init() { display.drawString (0, 16, "LORA"); display.display(); @@ -137,11 +187,11 @@ void lora_init() { LoRa.setCodingRate4(cfg.lora_coding_rate); LoRa.setSpreadingFactor(cfg.lora_spreading_factor); LoRa.setTxPower(cfg.lora_power, 1); - LoRa.setOCP(200); +// LoRa.setTxPower(cfg.lora_power); + LoRa.setOCP(250); LoRa.idle(); LoRa.onReceive(onReceive); LoRa.enableCrc(); - LoRa.receive(); } // ----------------------------------------------------------------------------- Display @@ -192,7 +242,10 @@ void display_draw() { display.drawString (110,30, String(sys_pps)); display.drawString (110,42, sys_rssi); -display.drawString (60,54, String(sys_msp_ticker)); +display.drawString (60,54, String(sys_tx_ticker)); + +display.drawString (85 + last_received_id * 8, 54, String(last_received_id)); + display.setTextAlignment (TEXT_ALIGN_LEFT); if (curr.gps.fixType == 1) display.drawString (33,7, "2D"); @@ -203,10 +256,10 @@ display.drawString (60,54, String(sys_msp_ticker)); display.drawString (33,42, "UAV"); display.drawString (116,16, "V"); display.drawString (112,30, "Hz"); - display.drawString (112,42, "dB"); + display.drawString (112,42, "dB"); - if (String(sys_curr_fc) != String("NO FC")) { - display.drawString (0,54, sys_curr_fc); + if (String(sys_curr_fc) != String("NOFC")) { + display.drawString (0,54, String(sys_curr_fc)); } // display.drawString (84, 54, display_lora_tx ? "TX" : " "); // display.drawString (106, 54, display_lora_rx ? "RX" : " "); @@ -218,29 +271,30 @@ display.drawString (60,54, String(sys_msp_ticker)); } else { display.setFont (ArialMT_Plain_10); - for (size_t i = 0; i <=2 ; i++) { + for (int i = 0; i < 3 ; i++) { if (peers[i].id != 0) { display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (0, i*16, peers[i].peer.name); + display.drawString (0, i * 16, String(peers[i].id) + ":"); + display.drawString (20, i * 16, peers[i].name); display.drawString (0, 8 + i * 16, "RSSI " + String(peers[i].rssi)); display.setTextAlignment (TEXT_ALIGN_RIGHT); - display.drawString (128, i * 16, String((float)peers[i].peer.gps.lat/10000000, 6)); - display.drawString (128, 8 + i * 16, String((float)peers[i].peer.gps.lon/10000000, 6)); + display.drawString (128, i * 16, String((float)peers[i].gps.lat/10000000, 6)); + display.drawString (128, 8 + i * 16, String((float)peers[i].gps.lon/10000000, 6)); } } } } - + else if (sys_display_page == 2) { display.setFont (ArialMT_Plain_10); for (size_t i = 0; i <=1 ; i++) { if (peers[i].id != 0) { display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (0, i*16, peers[i+3].peer.name); + display.drawString (0, i*16, peers[i+3].name); display.drawString (0, 8 + i * 16, "RSSI " + String(peers[i].rssi)); display.setTextAlignment (TEXT_ALIGN_RIGHT); - display.drawString (65, i * 16, String((float)peers[i+3].peer.gps.lat/10000000, 6)); - display.drawString (65, 8 + i * 16, String((float)peers[i+3].peer.gps.lon/10000000, 6)); + display.drawString (65, i * 16, String((float)peers[i+3].gps.lat/10000000, 6)); + display.drawString (65, 8 + i * 16, String((float)peers[i+3].gps.lon/10000000, 6)); } } } @@ -270,21 +324,26 @@ void msp_set_curr_vbat() { void msp_set_curr_name() { + /* char buff[16]; - if (!msp.request(MSP_NAME, &buff, sizeof(curr.name))) { - for (size_t i = 0; i < 5; i++) { - curr.name[i] = (char) random(65, 90); + + msp.request(MSP_NAME, &buff, sizeof(curr.name)); + + if (String(buff) != "") { + for (int i = 0; i < 5; i++) { + curr.name[i] = buff[i]; // ------ } } else { - for (size_t i = 0; i < 5; i++) { - curr.name[i] = buff[i]; // ------ - } - } +*/ + for (int i = 0; i < 5; i++) { + curr.name[i] = (char) random(65, 90); + } + // } } - -void msp_set_curr_fc() { + +void msp_set_curr_fc() { if (!msp.request(MSP_FC_VARIANT, &sys_curr_fc, sizeof(sys_curr_fc))) { - String("NoFC ").toCharArray(sys_curr_fc, 5); + String("NOFC").toCharArray(sys_curr_fc, 5); } } @@ -292,15 +351,15 @@ void msp_set_curr_fc() { void msp_send_peers() { msp_radar_pos_t radarPos; - for (size_t i = 0; i <= 4; i++) { + for (int i = 0; i < LORA_MAXPEERS; i++) { if (peers[i].id != 0) { radarPos.id = i; - radarPos.state = peers[i].peer.state; - radarPos.lat = peers[i].peer.gps.lat; - radarPos.lon = peers[i].peer.gps.lon; - radarPos.alt = peers[i].peer.gps.alt; - radarPos.speed = peers[i].peer.gps.groundSpeed; - radarPos.heading = peers[i].peer.gps.groundCourse / 10; + radarPos.state = peers[i].state; + radarPos.lat = peers[i].gps.lat; + radarPos.lon = peers[i].gps.lon; + radarPos.alt = peers[i].gps.alt; + radarPos.speed = peers[i].gps.groundSpeed; + radarPos.heading = peers[i].gps.groundCourse / 10; msp.command(MSP_SET_RADAR_POS, &radarPos, sizeof(radarPos)); } } @@ -311,7 +370,7 @@ void msp_init() { display.drawString (0, 24, "MSP "); display.display(); delay(100); - Serial1.begin(57600, SERIAL_8N1, cfg.msp_rx_pin , cfg.msp_tx_pin); + Serial1.begin(115200, SERIAL_8N1, cfg.msp_rx_pin , cfg.msp_tx_pin); msp.begin(Serial1); display.drawString (100, 24, "OK"); @@ -322,7 +381,7 @@ void msp_init() { delay(cfg.msp_fc_timeout * 1000); msp_set_curr_name(); msp_set_curr_fc(); - msp_set_curr_gps(); +// msp_set_curr_gps(); display.drawString (100, 32, sys_curr_fc); display.display(); @@ -355,8 +414,6 @@ void IRAM_ATTR handleInterrupt() { } void setup() { -// Serial.begin(115200); -// serialConsole[0] = &Serial; config_init(); @@ -364,126 +421,197 @@ void setup() { display.drawString(0, 8, "FIRMWARE"); display.drawString(100, 8, VERSION); display.display(); - + lora_init(); display.drawString (100, 16, "OK"); - display.display(); - - delay(500); - + display.display(); + msp_init(); delay(500); pinMode(interruptPin, INPUT); io_button_pressed = 0; attachInterrupt(digitalPinToInterrupt(interruptPin), handleInterrupt, RISING); - - for (size_t i = 0; i <= 4; i++) { + + + display.drawString (0, 40, "SCAN"); + display.display(); + + for (int i = 0; i < LORA_MAXPEERS; i++) { peers[i].id = 0; } - - curr.id = 0; - lora_mode = LORA_RX; - lora_cycle_eff = cfg.lora_cycle; + + curr.id = 0; lora_last_tx = millis(); main_updated = millis(); + display_updated = millis(); + cycle_scan_begin = millis(); + LoRa.sleep(); + LoRa.receive(); + + lora_mode = LORA_INIT; + } // ----------------------------------------------------------------------------- MAIN LOOP void loop() { + // ---------------------- IO BUTTON - if ((millis() - io_button_released) > 150 && io_button_pressed == 1) { + if (((millis() - io_button_released) > 150) && (io_button_pressed == 1)) { io_button_pressed = 0; } -// ---------------------- LORA INIT -/* - if (lora_mode == LORA_INIT && curr.id == 0) { - lora_mode = LORA_PICKUP; - sys_pickup_updated = millis(); - } -*/ -// ---------------------- LORA PICKUP -/* - if ((lora_mode == LORA_PICKUP) && (millis() - sys_pickup_updated > cfg.lora_pickup_delay * 1000)) { - sys_num_peers = 0; - for (int i = 0; i < sizeof(peers); i++) { - if (peers[i].id == 0 && curr.id == 0) { - curr.id = i + 1; - lora_mode = LORA_RX; +// ---------------------- LORA INIT + + if (lora_mode == LORA_INIT) { + + if (millis() > (cycle_scan_begin + cfg.cycle_scan)) { + + for (int i = 0; i < LORA_MAXPEERS; i++) { + if ((peers[i].id == 0)) { + if (curr.id == 0) { + curr.id = i + 1; + } + } else { + sys_num_peers++; + } } - if (peers[i].id != 0) { + lora_mode = LORA_START; + } else { + // delay (20); + + sys_num_peers = 0; + for (int i = 0; i < LORA_MAXPEERS; i++) { + if (peers[i].id > 0) { sys_num_peers++; + } } - } - if (peers[0].id == 1) { - lora_last_tx = peers[0].updated; + + if (sys_num_peers > scan_last_peers) { + display.drawString (40 + sys_num_peers * 8, 40, String(sys_num_peers)); + display.display(); + scan_last_peers = sys_num_peers; + } + } } -*/ + +// ---------------------- LORA START + + if (lora_mode == LORA_START) { + + if (sys_num_peers == 0 ) { // Alone, start at will + lora_last_tx = millis(); + } + else { // Not alone, sync by slot + + int gogogo = 0; + for (int i = 0; (i < LORA_MAXPEERS) && (gogogo == 0); i++) { + if (peers[i].id > 0) { + lora_last_tx = peers[i].updated + (curr.id - peers[i].id) * cfg.cycle_lora_slotspacing; + gogogo = 1; + } + } + + } + display_updated = millis(); + main_updated = millis(); + lora_mode = LORA_RX; + + } + + // ---------------------- LORA RX - - - if ((millis() - lora_last_tx) > lora_cycle_eff) { + + if ((lora_mode == LORA_RX) && (millis() > (lora_last_tx + cfg.cycle_lora))) { lora_mode = LORA_TX; - lora_cycle_eff = cfg.lora_cycle - random(0, cfg.lora_cycle_var); - lora_last_tx = millis(); } -// ---------------------- LORA TX - +// ---------------------- LORA TX + if (lora_mode == LORA_TX) { - - - // msp_set_curr_gps(); - - // if (curr.gps.fixType > 0) { - lora_send(&curr); - // } - + + +// if (curr.gps.fixType > 0) { + + if (String(sys_curr_fc) == "INAV" ) { + msp_set_curr_gps(); + } +// else { + // curr.gps.fixType = 0; + // } + + + if (sys_tx_ticker < 255) { + sys_tx_ticker++; + } + else { + sys_tx_ticker = 0; + } + + j.id = curr.id; + j.name[0] = curr.name[0]; + j.name[1] = curr.name[1]; + j.name[2] = curr.name[2]; + j.name[3] = curr.name[3]; + j.name[4] = curr.name[4]; +/* + j.header[0] = cfg.lora_header[0]; + j.header[1] = cfg.lora_header[1]; + j.header[2] = cfg.lora_header[2]; +*/ + j.tick = sys_tx_ticker; + j.lat = curr.gps.lat; + j.lon = curr.gps.lon; + j.alt = curr.gps.alt; + j.heading = curr.gps.groundCourse; + + lora_send(&j); + lora_last_tx = millis(); + lora_mode = LORA_RX; +// } + if (String(sys_curr_fc) == "INAV" ) { msp_send_peers(); } LoRa.sleep(); LoRa.receive(); - lora_mode = LORA_RX; } - - if ((display_enabled && ((millis() - display_updated)) > cfg.display_cycle)) { - + + if (((millis() - display_updated) > cfg.cycle_display) && display_enabled && (lora_mode > LORA_START)) { display_draw(); display_updated = millis(); } - - - if ((millis() - main_updated) > cfg.main_cycle) { + + + if ((millis() > (cfg.cycle_main + main_updated)) && (lora_mode > LORA_START)) { sys_num_peers = 0; sys_pps = sys_ppsc; sys_ppsc = 0; - - for (size_t i = 0; i <= 4; i++) { - if (peers[i].id != 0) { + + for (int i = 0; i < (LORA_MAXPEERS - 1); i++) { + if (peers[i].id > 0) { sys_num_peers++; } - if (peers[i].id != 0 && ((millis() - peers[i].updated) > cfg.peer_timeout * 1000)) { // Peer timeout - peers[i].peer.state = 0; + if (peers[i].id > 0 && ((millis() - peers[i].updated) > cfg.peer_timeout * 1000)) { // Peer timeout + peers[i].state = 0; // msp_send_peers(); peers[i].id = 0; sys_rssi = "0"; - String("").toCharArray(peers[i].peer.name, 5); + String("").toCharArray(peers[i].name, 5); } } @@ -492,11 +620,11 @@ void loop() { msp_set_curr_vbat(); } - if ((curr.state == 0) && (display_enabled == 0)) { + if ((curr.state == 0) && (display_enabled == 0)) { // Aircraft is disarmed = Turning on the OLED display_enable(); } - - else if (curr.state == 1 && (display_enabled == 1)) { // Aircraft is armed = Turning off the OLED + + else if ((curr.state == 1) && (display_enabled == 1)) { // Aircraft is armed = Turning off the OLED display_disable(); } diff --git a/src/main.h b/src/main.h index c154582..c529448 100644 --- a/src/main.h +++ b/src/main.h @@ -1,41 +1,66 @@ #define LORA_INIT 0 -#define LORA_PICKUP 1 +#define LORA_START 1 #define LORA_RX 2 #define LORA_TX 3 + struct config { - char lora_header[7]; + char lora_header[3]; uint32_t lora_frequency; uint32_t lora_bandwidth; uint8_t lora_coding_rate; uint8_t lora_spreading_factor; uint8_t lora_power; - uint8_t lora_pickup_delay; - uint16_t lora_cycle; - uint8_t lora_cycle_var; - uint16_t display_cycle; - uint16_t main_cycle; + uint16_t cycle_lora; + uint16_t cycle_lora_slotspacing; + uint16_t cycle_scan; + uint16_t cycle_display; + uint16_t cycle_main; uint16_t peer_timeout; uint8_t msp_fc_timeout; uint8_t msp_tx_pin; uint8_t msp_rx_pin; }; +/* struct peer_t { char header[5]; uint8_t id; - uint8_t tick; - char name[5]; uint8_t state; + char name[5]; + uint8_t tick; msp_raw_gps_t gps; }; struct peers_t { uint8_t id; - long updated; - double rssi; + uint32_t updated; + int16_t rssi; peer_t peer; }; +*/ +struct peer_t { + uint8_t id; + uint8_t state; + char name[5]; + char header[3]; + uint8_t tick; + uint32_t updated; + int16_t rssi; + msp_raw_gps_t gps; +}; + +struct peer_air_t { + uint8_t id; + char name[5]; +// char header[3]; + uint8_t tick; + int32_t lat; + int32_t lon; + int16_t alt; +// int16_t speed; + int16_t heading; +}; From 0cd949f836233350ceb6685715b9486c7a48728f Mon Sep 17 00:00:00 2001 From: Olivier C Date: Wed, 17 Apr 2019 13:10:31 +0200 Subject: [PATCH 03/17] Daily tweaks --- src/main.cpp | 335 ++++++++++++++++++++++----------------------------- src/main.h | 64 ++++------ 2 files changed, 170 insertions(+), 229 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index b5c616a..61ca29a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,44 +6,32 @@ #include #include -#define SCK 5 // GPIO5 - SX1278's SCK -#define MISO 19 // GPIO19 - SX1278's MISO -#define MOSI 27 // GPIO27 - SX1278's MOSI -#define SS 18 // GPIO18 - SX1278's CS -#define RST 14 // GPIO14 - SX1278's RESET -#define DI0 26 // GPIO26 - SX1278's IRQ (interrupt request) -#define VERSION "Z1" -#define LORA_MAXPEERS 5 + + // ----------------------------------------------------------------------------- global vars -config cfg; -MSP msp; -// Stream *serialConsole[1]; +MSP msp; int sys_display_page = 0; SSD1306 display(0x3c, 4, 15); -long sys_pickup_updated = 0; - -long lora_last_tx = 0; -long main_updated = 0; -long currentUpdateTime = 0; - msp_analog_t sys_curr_vbat; char sys_curr_fc[5]; peer_t curr; // Our peer -peer_t peers[5]; // Other peers +peer_t peers[LORA_MAXPEERS]; // Other peers peer_air_t incoming; // Received peer_air_t j; // Sent -int sys_tx_ticker = 0; +uint8_t sys_tx_ticker = 0; int sys_num_peers = -1; String sys_rssi = "0"; + + uint8_t sys_pps = 0; uint8_t sys_ppsc = 0; @@ -54,40 +42,23 @@ long io_button_released = 0; bool display_enabled = 1; long display_updated = 0; -long cycle_scan_begin; +long cycle_scan_begin; uint8_t scan_last_peers = -1; int lora_mode = LORA_INIT; int last_received_id = 0; -char last_received_name[5]; - -// ----------------------------------------------------------------------------- EEPROM / config - -void config_init() { - - String("R01").toCharArray(cfg.lora_header, 3); // protocol identifier - cfg.lora_frequency = 433E6; // 433E6, 868E6, 915E6 - cfg.lora_bandwidth = 250000;// KHz - cfg.lora_coding_rate = 5; // Error correction rate - cfg.lora_spreading_factor = 8; - cfg.lora_power = 20; //17 PA OFF, 18-20 PA ON +char last_received_name[7]; +int last_tx_duration = 0; +long last_tx_begin; +long last_tx_end; - cfg.cycle_lora = 480; // ms - cfg.cycle_lora_slotspacing = 96; // ms - cfg.cycle_scan = 4000; // ms - cfg.cycle_display = 150; // ms - cfg.cycle_main = 1000; // ms - - cfg.peer_timeout = 3; // sec - cfg.msp_fc_timeout = 2; // sec - - cfg.msp_tx_pin = 23; // pin for MSP serial TX - cfg.msp_rx_pin = 17; // pin for MSP serial RX -} +long lora_last_tx = 0; +long main_updated = 0; +long now_tx = 0; -// ----------------------------------------------------------------------------- LoRa +// -------- LoRa void lora_send(peer_air_t *outgoing) { @@ -97,14 +68,10 @@ void lora_send(peer_air_t *outgoing) { } - - void onReceive(int packetSize) { if (packetSize == 0) return; LoRa.readBytes((uint8_t *)&incoming, packetSize); - // if (String(incoming.header) == String(cfg.lora_header)) { // New data from a peer - sys_rssi = LoRa.packetRssi(); sys_ppsc++; /* @@ -118,20 +85,16 @@ void onReceive(int packetSize) { uint8_t id = incoming.id - 1; peers[id].id = incoming.id; last_received_id = incoming.id; - + peers[id].name[0] = incoming.name[0]; peers[id].name[1] = incoming.name[1]; peers[id].name[2] = incoming.name[2]; peers[id].name[3] = incoming.name[3]; peers[id].name[4] = incoming.name[4]; -/* - peers[id].header[0] = incoming.header[0]; - peers[id].header[1] = incoming.header[1]; - peers[id].header[2] = incoming.header[2]; -*/ + peers[id].tick = incoming.tick; peers[id].updated = millis(); - // peers[id].rssi = sys_rssi; +// peers[id].rssi = sys_rssi; peers[id].gps.lat = incoming.lat; peers[id].gps.lon = incoming.lon; peers[id].gps.alt = incoming.alt; @@ -140,54 +103,28 @@ void onReceive(int packetSize) { peers[id].gps.lon = 0; peers[id].gps.alt = 0; -// peers[id].gps.groundSpeed = incoming.speed; + peers[id].gps.groundSpeed = incoming.speed; peers[id].gps.groundCourse = incoming.heading; // } } - -/* -void onReceive(int packetSize) { - if (packetSize == 0) return; - LoRa.readBytes((uint8_t *)&incoming, packetSize); - - //if (String(incoming.header) == String(cfg.lora_header)) { // new plane data - - sys_rssi = String(LoRa.packetRssi()); - sys_ppsc++; - - - uint8_t id = incoming.id - 1; - peers[id].id = incoming.id; - - peers[id].rssi = LoRa.packetRssi(); - peers[id].state = 1; - peers[id].updated = millis(); -//} -} - -*/ - - void lora_init() { display.drawString (0, 16, "LORA"); display.display(); - String(cfg.lora_header).toCharArray(curr.header,5); SPI.begin(5, 19, 27, 18); LoRa.setPins(SS, RST, DI0); - if (!LoRa.begin(cfg.lora_frequency)) { + if (!LoRa.begin(LORA_FREQUENCY)) { display.drawString (94, 8, "FAIL"); while (1); } LoRa.sleep(); - LoRa.setSignalBandwidth(cfg.lora_bandwidth); - LoRa.setCodingRate4(cfg.lora_coding_rate); - LoRa.setSpreadingFactor(cfg.lora_spreading_factor); - LoRa.setTxPower(cfg.lora_power, 1); -// LoRa.setTxPower(cfg.lora_power); + LoRa.setSignalBandwidth(LORA_BANDWIDTH); + LoRa.setCodingRate4(LORA_CODING_RATE); + LoRa.setSpreadingFactor(LORA_SPREADING_FACTOR); + LoRa.setTxPower(LORA_POWER, 1); LoRa.setOCP(250); LoRa.idle(); LoRa.onReceive(onReceive); @@ -209,12 +146,6 @@ void display_init() { } void display_disable() { - display.clear(); - display.setFont (ArialMT_Plain_24); - display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (20, 20, "Armed"); - display.display(); - delay(250); display.clear(); display.display(); display_enabled = 0; @@ -222,11 +153,7 @@ void display_disable() { void display_enable() { display.clear(); - display.setFont (ArialMT_Plain_24); - display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (20,20, "Disarmed"); display.display(); - delay(250); display_enabled = 1; } @@ -241,12 +168,13 @@ void display_draw() { display.setFont (ArialMT_Plain_10); display.drawString (110,30, String(sys_pps)); display.drawString (110,42, sys_rssi); - -display.drawString (60,54, String(sys_tx_ticker)); - -display.drawString (85 + last_received_id * 8, 54, String(last_received_id)); - - + display.drawString (80, 42, String(sys_tx_ticker)); + display.drawString (36, 54, String(last_tx_duration)); + + if (last_received_id > 0) { + display.drawString (85 + last_received_id * 8, 54, String(last_received_id)); + } + display.setTextAlignment (TEXT_ALIGN_LEFT); if (curr.gps.fixType == 1) display.drawString (33,7, "2D"); if (curr.gps.fixType == 2) display.drawString (33,7, "3D"); @@ -261,8 +189,6 @@ display.drawString (85 + last_received_id * 8, 54, String(last_received_id)); if (String(sys_curr_fc) != String("NOFC")) { display.drawString (0,54, String(sys_curr_fc)); } -// display.drawString (84, 54, display_lora_tx ? "TX" : " "); -// display.drawString (106, 54, display_lora_rx ? "RX" : " "); } else if (sys_display_page == 1) { @@ -271,41 +197,69 @@ display.drawString (85 + last_received_id * 8, 54, String(last_received_id)); } else { display.setFont (ArialMT_Plain_10); - for (int i = 0; i < 3 ; i++) { + int j = 0; + for (int i = 0; (i < LORA_MAXPEERS) && (j < 4) ; i++) { if (peers[i].id != 0) { display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (0, i * 16, String(peers[i].id) + ":"); - display.drawString (20, i * 16, peers[i].name); - display.drawString (0, 8 + i * 16, "RSSI " + String(peers[i].rssi)); + display.drawString (0, j * 12, String(peers[i].id) + ":"); + display.drawString (20, j * 12, String(peers[i].name)); + // display.drawString (0, 8 + j * 16, "LQ " + String(peers[i].rssi)); display.setTextAlignment (TEXT_ALIGN_RIGHT); - display.drawString (128, i * 16, String((float)peers[i].gps.lat/10000000, 6)); - display.drawString (128, 8 + i * 16, String((float)peers[i].gps.lon/10000000, 6)); + // display.drawString (128, j * 16, String((float)peers[i].gps.lat/10000000, 6)); + // display.drawString (128, 8 + j * 16, String((float)peers[i].gps.lon/10000000, 6)); + + if (last_tx_begin > peers[i].updated) { + display.drawString (120, j * 12, String(last_tx_begin - peers[i].updated)); + display.drawString (128, j * 12, "-"); + } + else { + display.drawString (120, j * 12, String((peers[i].updated) - last_tx_begin)); + display.drawString (128, j * 12, "+"); + } + + + j++; } } } } - else if (sys_display_page == 2) { - display.setFont (ArialMT_Plain_10); - for (size_t i = 0; i <=1 ; i++) { - if (peers[i].id != 0) { - display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (0, i*16, peers[i+3].name); - display.drawString (0, 8 + i * 16, "RSSI " + String(peers[i].rssi)); - display.setTextAlignment (TEXT_ALIGN_RIGHT); - display.drawString (65, i * 16, String((float)peers[i+3].gps.lat/10000000, 6)); - display.drawString (65, 8 + i * 16, String((float)peers[i+3].gps.lon/10000000, 6)); + if (sys_num_peers == 0) { + display.drawString (0, 0, "No UAV detected"); } - } + else { + display.setFont (ArialMT_Plain_10); + display.setTextAlignment (TEXT_ALIGN_RIGHT); + display.drawString (128, 0 , "--------------------------------------0"); + int offset = 0; + int j = 1; + for (int i = 0; i < LORA_MAXPEERS ; i++) { + if (peers[i].id != 0) { + offset = 128 - (120 * (peers[i].updated - last_tx_begin) / 500); + + display.drawString (offset, 0, String(peers[i].id)); + + if (j < 5) { + if (last_tx_begin > peers[i].updated) { + display.drawString (offset + 16, j * 12, String(last_tx_begin - peers[i].updated)); + } + else { + display.drawString (offset + 16, j * 12, String((peers[i].updated) - last_tx_begin)); + } + } + + j++; + + } + } + } } + last_received_id = 0; display.display(); } - - -// ----------------------------------------------------------------------------- MSP and FC - +// -------- MSP and FC void msp_set_curr_state() { @@ -322,13 +276,12 @@ void msp_set_curr_vbat() { msp.request(MSP_ANALOG, &sys_curr_vbat, sizeof(sys_curr_vbat)); } - void msp_set_curr_name() { /* char buff[16]; msp.request(MSP_NAME, &buff, sizeof(curr.name)); - + if (String(buff) != "") { for (int i = 0; i < 5; i++) { curr.name[i] = buff[i]; // ------ @@ -338,6 +291,7 @@ void msp_set_curr_name() { for (int i = 0; i < 5; i++) { curr.name[i] = (char) random(65, 90); } + curr.name[5] = 0; // } } @@ -352,7 +306,7 @@ void msp_set_curr_fc() { void msp_send_peers() { msp_radar_pos_t radarPos; for (int i = 0; i < LORA_MAXPEERS; i++) { - if (peers[i].id != 0) { + if (peers[i].id > 0) { radarPos.id = i; radarPos.state = peers[i].state; radarPos.lat = peers[i].gps.lat; @@ -370,7 +324,7 @@ void msp_init() { display.drawString (0, 24, "MSP "); display.display(); delay(100); - Serial1.begin(115200, SERIAL_8N1, cfg.msp_rx_pin , cfg.msp_tx_pin); + Serial1.begin(115200, SERIAL_8N1, SERIAL_PIN_RX , SERIAL_PIN_TX); msp.begin(Serial1); display.drawString (100, 24, "OK"); @@ -378,10 +332,9 @@ void msp_init() { display.drawString (0, 32, "FC "); display.display(); - delay(cfg.msp_fc_timeout * 1000); + delay(SERIAL_FC_TIMEOUT); msp_set_curr_name(); msp_set_curr_fc(); -// msp_set_curr_gps(); display.drawString (100, 32, sys_curr_fc); display.display(); @@ -401,7 +354,7 @@ void IRAM_ATTR handleInterrupt() { if (io_button_pressed == 0) { io_button_pressed = 1; - if (sys_display_page >= 1) { + if (sys_display_page >= 2) { sys_display_page = 0; } else { @@ -415,8 +368,6 @@ void IRAM_ATTR handleInterrupt() { void setup() { - config_init(); - display_init(); display.drawString(0, 8, "FIRMWARE"); display.drawString(100, 8, VERSION); @@ -425,8 +376,8 @@ void setup() { lora_init(); display.drawString (100, 16, "OK"); - display.display(); - + display.display(); + msp_init(); delay(500); @@ -434,7 +385,6 @@ void setup() { io_button_pressed = 0; attachInterrupt(digitalPinToInterrupt(interruptPin), handleInterrupt, RISING); - display.drawString (0, 40, "SCAN"); display.display(); @@ -442,8 +392,8 @@ void setup() { peers[i].id = 0; } - curr.id = 0; - + curr.id = 0; + lora_last_tx = millis(); main_updated = millis(); @@ -452,9 +402,9 @@ void setup() { LoRa.sleep(); LoRa.receive(); - + lora_mode = LORA_INIT; - + } // ----------------------------------------------------------------------------- MAIN LOOP @@ -464,7 +414,7 @@ void loop() { // ---------------------- IO BUTTON - if (((millis() - io_button_released) > 150) && (io_button_pressed == 1)) { + if ((millis() > io_button_released + 150) && (io_button_pressed == 1)) { io_button_pressed = 0; } @@ -472,7 +422,7 @@ void loop() { if (lora_mode == LORA_INIT) { - if (millis() > (cycle_scan_begin + cfg.cycle_scan)) { + if (millis() > (cycle_scan_begin + CYCLE_SCAN)) { for (int i = 0; i < LORA_MAXPEERS; i++) { if ((peers[i].id == 0)) { @@ -486,20 +436,20 @@ void loop() { lora_mode = LORA_START; } else { // delay (20); - + sys_num_peers = 0; for (int i = 0; i < LORA_MAXPEERS; i++) { if (peers[i].id > 0) { sys_num_peers++; } } - + if (sys_num_peers > scan_last_peers) { display.drawString (40 + sys_num_peers * 8, 40, String(sys_num_peers)); display.display(); scan_last_peers = sys_num_peers; } - + } } @@ -512,10 +462,10 @@ void loop() { } else { // Not alone, sync by slot - int gogogo = 0; + bool gogogo = 0; for (int i = 0; (i < LORA_MAXPEERS) && (gogogo == 0); i++) { if (peers[i].id > 0) { - lora_last_tx = peers[i].updated + (curr.id - peers[i].id) * cfg.cycle_lora_slotspacing; + lora_last_tx = peers[i].updated + (curr.id - peers[i].id) * LORA_CYCLE_SLOTSPACING; gogogo = 1; } } @@ -530,7 +480,7 @@ void loop() { // ---------------------- LORA RX - if ((lora_mode == LORA_RX) && (millis() > (lora_last_tx + cfg.cycle_lora))) { + if ((lora_mode == LORA_RX) && (millis() > (lora_last_tx + LORA_CYCLE))) { lora_mode = LORA_TX; } @@ -539,46 +489,49 @@ void loop() { if (lora_mode == LORA_TX) { - - -// if (curr.gps.fixType > 0) { - + now_tx = millis(); + if (String(sys_curr_fc) == "INAV" ) { msp_set_curr_gps(); - } -// else { - // curr.gps.fixType = 0; - // } + if (curr.gps.fixType > 0) { + j.lat = curr.gps.lat; + j.lon = curr.gps.lon; + j.alt = curr.gps.alt; + j.heading = curr.gps.groundCourse; + j.speed = curr.gps.groundSpeed; + } + else { + j.lat = 0; + j.lon = 0; + j.alt = 0; + j.heading = 0; + j.speed = 0; + } + } - if (sys_tx_ticker < 255) { - sys_tx_ticker++; - } - else { - sys_tx_ticker = 0; - } + if (sys_tx_ticker < 255) { + sys_tx_ticker++; + } + else { + sys_tx_ticker = 0; + } - j.id = curr.id; - j.name[0] = curr.name[0]; - j.name[1] = curr.name[1]; - j.name[2] = curr.name[2]; - j.name[3] = curr.name[3]; - j.name[4] = curr.name[4]; -/* - j.header[0] = cfg.lora_header[0]; - j.header[1] = cfg.lora_header[1]; - j.header[2] = cfg.lora_header[2]; -*/ - j.tick = sys_tx_ticker; - j.lat = curr.gps.lat; - j.lon = curr.gps.lon; - j.alt = curr.gps.alt; - j.heading = curr.gps.groundCourse; - - lora_send(&j); - lora_last_tx = millis(); - lora_mode = LORA_RX; -// } + j.id = curr.id; + j.name[0] = curr.name[0]; + j.name[1] = curr.name[1]; + j.name[2] = curr.name[2]; + j.name[3] = curr.name[3]; + j.name[4] = curr.name[4]; + j.name[5] = curr.name[5]; + j.name[6] = curr.name[6]; + j.tick = sys_tx_ticker; + + last_tx_begin = millis(); + lora_send(&j); + lora_last_tx = millis(); + last_tx_duration = lora_last_tx - last_tx_begin; + lora_mode = LORA_RX; if (String(sys_curr_fc) == "INAV" ) { msp_send_peers(); @@ -590,13 +543,13 @@ void loop() { } - if (((millis() - display_updated) > cfg.cycle_display) && display_enabled && (lora_mode > LORA_START)) { + if ((millis() > display_updated + CYCLE_DISPLAY) && display_enabled && (lora_mode > LORA_START)) { display_draw(); display_updated = millis(); } - if ((millis() > (cfg.cycle_main + main_updated)) && (lora_mode > LORA_START)) { + if ((millis() > (CYCLE_MAIN + main_updated)) && (lora_mode > LORA_START)) { sys_num_peers = 0; sys_pps = sys_ppsc; sys_ppsc = 0; @@ -606,12 +559,12 @@ void loop() { sys_num_peers++; } - if (peers[i].id > 0 && ((millis() - peers[i].updated) > cfg.peer_timeout * 1000)) { // Peer timeout + if (peers[i].id > 0 && ((millis() - peers[i].updated) > LORA_PEER_TIMEOUT)) { // Peer timeout peers[i].state = 0; - // msp_send_peers(); + peers[i].id = 0; sys_rssi = "0"; - String("").toCharArray(peers[i].name, 5); + String("").toCharArray(peers[i].name, 7); } } diff --git a/src/main.h b/src/main.h index c529448..9d9e2fb 100644 --- a/src/main.h +++ b/src/main.h @@ -1,51 +1,40 @@ +#define VERSION "O1" #define LORA_INIT 0 #define LORA_START 1 #define LORA_RX 2 #define LORA_TX 3 +#define LORA_FREQUENCY 433E6 +#define LORA_BANDWIDTH 250000 +#define LORA_CODING_RATE 5 +#define LORA_SPREADING_FACTOR 7 +#define LORA_POWER 20 +#define LORA_CYCLE 480 // ms +#define LORA_CYCLE_SLOTSPACING 96 -struct config { - char lora_header[3]; - uint32_t lora_frequency; - uint32_t lora_bandwidth; - uint8_t lora_coding_rate; - uint8_t lora_spreading_factor; - uint8_t lora_power; - uint16_t cycle_lora; - uint16_t cycle_lora_slotspacing; - uint16_t cycle_scan; - uint16_t cycle_display; - uint16_t cycle_main; - uint16_t peer_timeout; - uint8_t msp_fc_timeout; - uint8_t msp_tx_pin; - uint8_t msp_rx_pin; -}; +#define SERIAL_PIN_TX 23 +#define SERIAL_PIN_RX 17 -/* -struct peer_t { - char header[5]; - uint8_t id; - uint8_t state; - char name[5]; - uint8_t tick; - msp_raw_gps_t gps; -}; +#define CYCLE_SCAN 4000 // ms +#define CYCLE_DISPLAY 200 // ms +#define CYCLE_MAIN 1000 // ms -struct peers_t { - uint8_t id; - uint32_t updated; - int16_t rssi; - peer_t peer; -}; -*/ +#define LORA_MAXPEERS 5 +#define LORA_PEER_TIMEOUT 3000 // ms +#define SERIAL_FC_TIMEOUT 3000 // ms + +#define SCK 5 // GPIO5 - SX1278's SCK +#define MISO 19 // GPIO19 - SX1278's MISO +#define MOSI 27 // GPIO27 - SX1278's MOSI +#define SS 18 // GPIO18 - SX1278's CS +#define RST 14 // GPIO14 - SX1278's RESET +#define DI0 26 // GPIO26 - SX1278's IRQ (interrupt request) struct peer_t { uint8_t id; uint8_t state; - char name[5]; - char header[3]; + char name[7]; uint8_t tick; uint32_t updated; int16_t rssi; @@ -54,13 +43,12 @@ struct peer_t { struct peer_air_t { uint8_t id; - char name[5]; -// char header[3]; + char name[7]; uint8_t tick; int32_t lat; int32_t lon; int16_t alt; -// int16_t speed; + int16_t speed; int16_t heading; }; From 29df02a7bd603cc76a16c78c7ac8917c533e7bf4 Mon Sep 17 00:00:00 2001 From: Olivier C Date: Wed, 17 Apr 2019 21:13:49 +0200 Subject: [PATCH 04/17] Timings and timeline --- src/main.cpp | 200 +++++++++++++++++++++++++++------------------------ src/main.h | 4 +- 2 files changed, 111 insertions(+), 93 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 61ca29a..7852ffd 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -23,19 +23,16 @@ char sys_curr_fc[5]; peer_t curr; // Our peer peer_t peers[LORA_MAXPEERS]; // Other peers peer_air_t incoming; // Received -peer_air_t j; // Sent +peer_air_t peerout; // Sent uint8_t sys_tx_ticker = 0; int sys_num_peers = -1; String sys_rssi = "0"; - - uint8_t sys_pps = 0; uint8_t sys_ppsc = 0; -bool io_button_state = 1; // Not needed ? bool io_button_pressed = 0; long io_button_released = 0; @@ -56,7 +53,8 @@ long last_tx_end; long lora_last_tx = 0; long main_updated = 0; -long now_tx = 0; +long cycle_adjust_timing = 0; +bool collision = 0; // -------- LoRa @@ -170,11 +168,11 @@ void display_draw() { display.drawString (110,42, sys_rssi); display.drawString (80, 42, String(sys_tx_ticker)); display.drawString (36, 54, String(last_tx_duration)); - + if (last_received_id > 0) { display.drawString (85 + last_received_id * 8, 54, String(last_received_id)); } - + display.setTextAlignment (TEXT_ALIGN_LEFT); if (curr.gps.fixType == 1) display.drawString (33,7, "2D"); if (curr.gps.fixType == 2) display.drawString (33,7, "3D"); @@ -192,67 +190,67 @@ void display_draw() { } else if (sys_display_page == 1) { - if (sys_num_peers == 0) { - display.drawString (0, 0, "No UAV detected"); - } - else { - display.setFont (ArialMT_Plain_10); - int j = 0; - for (int i = 0; (i < LORA_MAXPEERS) && (j < 4) ; i++) { - if (peers[i].id != 0) { - display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (0, j * 12, String(peers[i].id) + ":"); - display.drawString (20, j * 12, String(peers[i].name)); - // display.drawString (0, 8 + j * 16, "LQ " + String(peers[i].rssi)); - display.setTextAlignment (TEXT_ALIGN_RIGHT); - // display.drawString (128, j * 16, String((float)peers[i].gps.lat/10000000, 6)); - // display.drawString (128, 8 + j * 16, String((float)peers[i].gps.lon/10000000, 6)); - - if (last_tx_begin > peers[i].updated) { - display.drawString (120, j * 12, String(last_tx_begin - peers[i].updated)); - display.drawString (128, j * 12, "-"); - } - else { - display.drawString (120, j * 12, String((peers[i].updated) - last_tx_begin)); - display.drawString (128, j * 12, "+"); - } - - - j++; + display.setFont (ArialMT_Plain_10); + display.setTextAlignment (TEXT_ALIGN_LEFT); + display.drawString (0, 1 , "_____________________"); + display.drawString (3, 4 , "."); + display.drawString (60, 4 , "."); + display.drawString (122, 4 , "."); + display.drawString (0, 14 , String(LORA_CYCLE)); + display.drawString (52, 14 , String(LORA_CYCLE / 2)); + display.drawString (120, 14 , "0"); + + int j = 0; + long pos[5]; + long tl_begin = millis(); + long diff; + int line; + + for (int i = 0; i < LORA_MAXPEERS ; i++) { + if (peers[i].id != 0) { + diff = tl_begin - peers[i].updated; + if ( diff < 480) { + pos[i] = 120 - diff / 4; } } + else { + pos[i] = 0; + } } - } - else if (sys_display_page == 2) { - if (sys_num_peers == 0) { - display.drawString (0, 0, "No UAV detected"); + + diff = tl_begin - lora_last_tx; + if ( diff < 480) { + display.drawString (120 - diff / 4, 0, "0"); + } + + for (int i = 0; i < LORA_MAXPEERS ; i++) { + if (pos[i] > 0) { + display.drawString (pos[i], 0, String(peers[i].id)); } - else { - display.setFont (ArialMT_Plain_10); - display.setTextAlignment (TEXT_ALIGN_RIGHT); - display.drawString (128, 0 , "--------------------------------------0"); - int offset = 0; - int j = 1; - for (int i = 0; i < LORA_MAXPEERS ; i++) { - if (peers[i].id != 0) { - offset = 128 - (120 * (peers[i].updated - last_tx_begin) / 500); - - display.drawString (offset, 0, String(peers[i].id)); - - if (j < 5) { - if (last_tx_begin > peers[i].updated) { - display.drawString (offset + 16, j * 12, String(last_tx_begin - peers[i].updated)); - } - else { - display.drawString (offset + 16, j * 12, String((peers[i].updated) - last_tx_begin)); - } - } - j++; - + if (peers[i].id != 0 && j < 4) { + line = j * 9 + 24; + display.setTextAlignment (TEXT_ALIGN_LEFT); + display.drawString (0, line, String(peers[i].id) + ":"); + display.drawString (16, line, String(peers[i].name)); + display.setTextAlignment (TEXT_ALIGN_RIGHT); + + if (last_tx_begin > peers[i].updated) { + display.drawString (120, line, String(last_tx_begin - peers[i].updated)); + display.drawString (128, line, "-"); } + else { + display.drawString (120, line, String((peers[i].updated) - last_tx_begin)); + display.drawString (128, line, "+"); + } + j++; } - } + } + } + else if (sys_display_page == 2) { + + + } last_received_id = 0; display.display(); @@ -354,7 +352,7 @@ void IRAM_ATTR handleInterrupt() { if (io_button_pressed == 0) { io_button_pressed = 1; - if (sys_display_page >= 2) { + if (sys_display_page >= 1) { sys_display_page = 0; } else { @@ -435,7 +433,6 @@ void loop() { } lora_mode = LORA_START; } else { - // delay (20); sys_num_peers = 0; for (int i = 0; i < LORA_MAXPEERS; i++) { @@ -451,13 +448,15 @@ void loop() { } } + sys_pps = 0; + sys_ppsc = 0; } // ---------------------- LORA START if (lora_mode == LORA_START) { - if (sys_num_peers == 0 ) { // Alone, start at will + if (sys_num_peers == 0) { // Alone, start at will lora_last_tx = millis(); } else { // Not alone, sync by slot @@ -465,7 +464,7 @@ void loop() { bool gogogo = 0; for (int i = 0; (i < LORA_MAXPEERS) && (gogogo == 0); i++) { if (peers[i].id > 0) { - lora_last_tx = peers[i].updated + (curr.id - peers[i].id) * LORA_CYCLE_SLOTSPACING; + lora_last_tx = peers[i].updated + (curr.id - peers[i].id) * LORA_CYCLE_SLOTSPACING + LORA_CYCLE_TIMING_DELAY; gogogo = 1; } } @@ -474,14 +473,16 @@ void loop() { display_updated = millis(); main_updated = millis(); lora_mode = LORA_RX; - + sys_pps = 0; + sys_ppsc = 0; } // ---------------------- LORA RX - if ((lora_mode == LORA_RX) && (millis() > (lora_last_tx + LORA_CYCLE))) { + if ((lora_mode == LORA_RX) && (millis() > (lora_last_tx + LORA_CYCLE + cycle_adjust_timing))) { lora_mode = LORA_TX; + cycle_adjust_timing = 0; } @@ -489,24 +490,22 @@ void loop() { if (lora_mode == LORA_TX) { - now_tx = millis(); - if (String(sys_curr_fc) == "INAV" ) { msp_set_curr_gps(); if (curr.gps.fixType > 0) { - j.lat = curr.gps.lat; - j.lon = curr.gps.lon; - j.alt = curr.gps.alt; - j.heading = curr.gps.groundCourse; - j.speed = curr.gps.groundSpeed; + peerout.lat = curr.gps.lat; + peerout.lon = curr.gps.lon; + peerout.alt = curr.gps.alt; + peerout.heading = curr.gps.groundCourse; + peerout.speed = curr.gps.groundSpeed; } else { - j.lat = 0; - j.lon = 0; - j.alt = 0; - j.heading = 0; - j.speed = 0; + peerout.lat = 0; + peerout.lon = 0; + peerout.alt = 0; + peerout.heading = 0; + peerout.speed = 0; } } @@ -517,22 +516,40 @@ void loop() { sys_tx_ticker = 0; } - j.id = curr.id; - j.name[0] = curr.name[0]; - j.name[1] = curr.name[1]; - j.name[2] = curr.name[2]; - j.name[3] = curr.name[3]; - j.name[4] = curr.name[4]; - j.name[5] = curr.name[5]; - j.name[6] = curr.name[6]; - j.tick = sys_tx_ticker; + peerout.id = curr.id; + peerout.name[0] = curr.name[0]; + peerout.name[1] = curr.name[1]; + peerout.name[2] = curr.name[2]; + peerout.name[3] = curr.name[3]; + peerout.name[4] = curr.name[4]; + peerout.name[5] = curr.name[5]; + peerout.name[6] = curr.name[6]; + peerout.tick = sys_tx_ticker; last_tx_begin = millis(); - lora_send(&j); + lora_send(&peerout); lora_last_tx = millis(); last_tx_duration = lora_last_tx - last_tx_begin; lora_mode = LORA_RX; + if (sys_num_peers > 0) { + for (int i = 0; i < LORA_MAXPEERS; i++) { + if (peers[i].id > 0) { + if (lora_last_tx - peers[i].updated < LORA_CYCLE_ANTICOLLISION) { + cycle_adjust_timing = LORA_CYCLE_ANTICOLLISION; + } + } + } + } + + /* + if (curr.id > 1) { + int prev = curr.id - 1; + if ((peers[prev].id > 1) && (peers[prev].updated > 0) && ((lora_last_tx - peers[prev].updated) < (LORA_CYCLE_SLOTSPACING / 2))) { + cycle_adjust_timing = LORA_CYCLE_SLOTSPACING / 2; + } + } +*/ if (String(sys_curr_fc) == "INAV" ) { msp_send_peers(); } @@ -554,14 +571,13 @@ void loop() { sys_pps = sys_ppsc; sys_ppsc = 0; - for (int i = 0; i < (LORA_MAXPEERS - 1); i++) { + for (int i = 0; i < LORA_MAXPEERS; i++) { if (peers[i].id > 0) { sys_num_peers++; } - if (peers[i].id > 0 && ((millis() - peers[i].updated) > LORA_PEER_TIMEOUT)) { // Peer timeout + if (peers[i].id > 0 && ((millis() - peers[i].updated) > LORA_PEER_TIMEOUT)) { peers[i].state = 0; - peers[i].id = 0; sys_rssi = "0"; String("").toCharArray(peers[i].name, 7); diff --git a/src/main.h b/src/main.h index 9d9e2fb..d7a90b9 100644 --- a/src/main.h +++ b/src/main.h @@ -11,7 +11,9 @@ #define LORA_SPREADING_FACTOR 7 #define LORA_POWER 20 #define LORA_CYCLE 480 // ms -#define LORA_CYCLE_SLOTSPACING 96 +#define LORA_CYCLE_SLOTSPACING 96 // ms +#define LORA_CYCLE_TIMING_DELAY -10 // ms +#define LORA_CYCLE_ANTICOLLISION 30 // ms #define SERIAL_PIN_TX 23 #define SERIAL_PIN_RX 17 From 0d749c5383cd9d7212494344e30352fb65248d53 Mon Sep 17 00:00:00 2001 From: Olivier C Date: Thu, 18 Apr 2019 22:03:05 +0200 Subject: [PATCH 05/17] Major changes 2 --- src/main.cpp | 338 ++++++++++++++++++++++++--------------------------- src/main.h | 37 ++++-- 2 files changed, 187 insertions(+), 188 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 7852ffd..0647caf 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -7,28 +7,24 @@ #include - - - // ----------------------------------------------------------------------------- global vars MSP msp; +stats_t stat; int sys_display_page = 0; SSD1306 display(0x3c, 4, 15); -msp_analog_t sys_curr_vbat; -char sys_curr_fc[5]; - -peer_t curr; // Our peer +curr_t curr; // Our peer peer_t peers[LORA_MAXPEERS]; // Other peers -peer_air_t incoming; // Received -peer_air_t peerout; // Sent +peer_air_t incoming; // Received peer +peer_air_t peerout; // Sent peer + -uint8_t sys_tx_ticker = 0; int sys_num_peers = -1; -String sys_rssi = "0"; +// String sys_rssi = "0"; +int sys_rssi; uint8_t sys_pps = 0; uint8_t sys_ppsc = 0; @@ -45,16 +41,15 @@ uint8_t scan_last_peers = -1; int lora_mode = LORA_INIT; int last_received_id = 0; -char last_received_name[7]; int last_tx_duration = 0; long last_tx_begin; long last_tx_end; long lora_last_tx = 0; +long lora_last_rx = 0; long main_updated = 0; long cycle_adjust_timing = 0; -bool collision = 0; // -------- LoRa @@ -68,53 +63,42 @@ void lora_send(peer_air_t *outgoing) { void onReceive(int packetSize) { if (packetSize == 0) return; + lora_last_rx = millis(); LoRa.readBytes((uint8_t *)&incoming, packetSize); - - sys_rssi = LoRa.packetRssi(); - sys_ppsc++; -/* - if (String(incoming.name) == peers[incoming.id].name) { // Known peer - peers[incoming.id].state = 1; - } - else { // Something wrong, id slot / name mismatch - peers[incoming.id].state = 1; // TODO, for now overwrite - } -*/ - uint8_t id = incoming.id - 1; - peers[id].id = incoming.id; - last_received_id = incoming.id; - - peers[id].name[0] = incoming.name[0]; - peers[id].name[1] = incoming.name[1]; - peers[id].name[2] = incoming.name[2]; - peers[id].name[3] = incoming.name[3]; - peers[id].name[4] = incoming.name[4]; - - peers[id].tick = incoming.tick; - peers[id].updated = millis(); -// peers[id].rssi = sys_rssi; - peers[id].gps.lat = incoming.lat; - peers[id].gps.lon = incoming.lon; - peers[id].gps.alt = incoming.alt; - - peers[id].gps.lat = 0; - peers[id].gps.lon = 0; - peers[id].gps.alt = 0; - - peers[id].gps.groundSpeed = incoming.speed; - peers[id].gps.groundCourse = incoming.heading; - // } + sys_rssi = LoRa.packetRssi(); + sys_ppsc++; + + uint8_t id = incoming.id - 1; + +// if (String(incoming.name) != String(peers[id].name)) { // Something wrong, id slot / name mismatch +// peers[incoming.id].state = 3; +// } + + peers[id].id = incoming.id; + last_received_id = incoming.id; + + strncpy(peers[id].name, incoming.name, 3); + peers[id].name[3] = 0; + + peers[id].tick = incoming.tick; + peers[id].updated = lora_last_rx; + peers[id].rssi = sys_rssi; + peers[id].gps.lat = incoming.lat; + peers[id].gps.lon = incoming.lon; + peers[id].gps.alt = incoming.alt; + peers[id].gps.groundSpeed = incoming.speed; + peers[id].gps.groundCourse = incoming.heading; } void lora_init() { - display.drawString (0, 16, "LORA"); + display.drawString (0, 9, "LORA"); display.display(); SPI.begin(5, 19, 27, 18); LoRa.setPins(SS, RST, DI0); if (!LoRa.begin(LORA_FREQUENCY)) { - display.drawString (94, 8, "FAIL"); + display.drawString (94, 9, "FAIL"); while (1); } @@ -144,49 +128,66 @@ void display_init() { } void display_disable() { - display.clear(); - display.display(); +// display.clear(); +// display.display(); + display.displayOff(); display_enabled = 0; } void display_enable() { - display.clear(); - display.display(); +// display.clear(); +// display.display(); + display.displayOn(); display_enabled = 1; } void display_draw() { display.clear(); if (sys_display_page == 0) { - display.setFont (ArialMT_Plain_24); - display.setTextAlignment (TEXT_ALIGN_RIGHT); - display.drawString (30,5, String(curr.gps.numSat)); - display.drawString (30,30, String(sys_num_peers)); - display.drawString (114,5, String((float)sys_curr_vbat.vbat/10)); - display.setFont (ArialMT_Plain_10); - display.drawString (110,30, String(sys_pps)); - display.drawString (110,42, sys_rssi); - display.drawString (80, 42, String(sys_tx_ticker)); - display.drawString (36, 54, String(last_tx_duration)); + + display.setFont(ArialMT_Plain_24); + display.setTextAlignment(TEXT_ALIGN_RIGHT); + display.drawString(26, 0, String(curr.gps.numSat)); + display.drawString(13, 42, String(sys_num_peers)); + // display.drawString(119, 0, String((float)curr.vbat / 10)); + + display.setFont(ArialMT_Plain_10); + display.drawString (126, 23, "_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ "); + display.drawString(107, 35, String(sys_pps)); + display.drawString(65, 45, String(sys_rssi)); +// display.drawString (80, 44, String(curr.tick)); + display.drawString (107, 45, String(stat.percent_received)); + display.drawString (65, 35, String(last_tx_duration)); + display.setTextAlignment (TEXT_ALIGN_LEFT); + display.drawString (60, 21, String(curr.name)); + display.drawString (27, 12, "SAT"); + display.drawString (108, 45, "%E"); + display.drawString (15, 45, "ID"); + display.drawString (28, 45, String(curr.id)); +// display.drawString (120,12, "V"); + display.drawString (66, 35, "ms"); + display.drawString (109, 35, "PS"); + display.drawString (66, 45, "dB"); + + display.drawString (0, 21, "FC:" + String(fc_name[curr.fc])); + if (last_received_id > 0) { - display.drawString (85 + last_received_id * 8, 54, String(last_received_id)); + display.drawString (48 + last_received_id * 8, 54, String(last_received_id)); } - display.setTextAlignment (TEXT_ALIGN_LEFT); + if (sys_num_peers == 0) { + display.drawString (15, 54, "Alone"); + } + else if (sys_num_peers == 1) { + display.drawString (15, 54, "Peer"); + } + else { + display.drawString (15, 54, "Peers"); + } + if (curr.gps.fixType == 1) display.drawString (33,7, "2D"); - if (curr.gps.fixType == 2) display.drawString (33,7, "3D"); - display.drawString (33,16, "SAT"); - display.drawString (33,34, "ID"); - display.drawString (49,34, String(curr.id)); - display.drawString (33,42, "UAV"); - display.drawString (116,16, "V"); - display.drawString (112,30, "Hz"); - display.drawString (112,42, "dB"); - - if (String(sys_curr_fc) != String("NOFC")) { - display.drawString (0,54, String(sys_curr_fc)); - } + if (curr.gps.fixType == 2) display.drawString (33,7, "3D"); } else if (sys_display_page == 1) { @@ -201,7 +202,7 @@ void display_draw() { display.drawString (120, 14 , "0"); int j = 0; - long pos[5]; + long pos[LORA_MAXPEERS]; long tl_begin = millis(); long diff; int line; @@ -246,59 +247,42 @@ void display_draw() { j++; } } - } - else if (sys_display_page == 2) { - - - } last_received_id = 0; display.display(); } - // -------- MSP and FC -void msp_set_curr_state() { +void msp_set_state() { uint32_t planeModes; msp.getActiveModes(&planeModes); curr.state = bitRead(planeModes, 0); } -void msp_set_curr_gps() { - msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); -} - -void msp_set_curr_vbat() { - msp.request(MSP_ANALOG, &sys_curr_vbat, sizeof(sys_curr_vbat)); -} - -void msp_set_curr_name() { - /* - char buff[16]; - - msp.request(MSP_NAME, &buff, sizeof(curr.name)); - - if (String(buff) != "") { - for (int i = 0; i < 5; i++) { - curr.name[i] = buff[i]; // ------ - } - } else { -*/ - for (int i = 0; i < 5; i++) { +void msp_set_name() { + if (msp.request(MSP_NAME, &curr.name, sizeof(curr.name))) { + // + } + else { + for (int i = 0; i < 3; i++) { curr.name[i] = (char) random(65, 90); } - curr.name[5] = 0; - // } -} - -void msp_set_curr_fc() { - if (!msp.request(MSP_FC_VARIANT, &sys_curr_fc, sizeof(sys_curr_fc))) { - String("NOFC").toCharArray(sys_curr_fc, 5); + curr.name[3] = 0; } + } +void msp_set_fc() { + char j[5]; + curr.fc = FC_NONE; + msp.request(MSP_FC_VARIANT, &j, sizeof(j)); + + if (strcmp(j, "INAV") == 0) { + curr.fc = FC_INAV; + } + } void msp_send_peers() { @@ -317,27 +301,6 @@ void msp_send_peers() { } } -void msp_init() { - - display.drawString (0, 24, "MSP "); - display.display(); - delay(100); - Serial1.begin(115200, SERIAL_8N1, SERIAL_PIN_RX , SERIAL_PIN_TX); - msp.begin(Serial1); - - display.drawString (100, 24, "OK"); - display.display(); - display.drawString (0, 32, "FC "); - display.display(); - - delay(SERIAL_FC_TIMEOUT); - msp_set_curr_name(); - msp_set_curr_fc(); - - display.drawString (100, 32, sys_curr_fc); - display.display(); -} - // ----------------------------------------------------------------------------- main init const byte interruptPin = 0; @@ -367,23 +330,40 @@ void IRAM_ATTR handleInterrupt() { void setup() { display_init(); - display.drawString(0, 8, "FIRMWARE"); - display.drawString(100, 8, VERSION); + display.drawString(0, 0, "RADAR VERSION"); + display.drawString(90, 0, VERSION); display.display(); lora_init(); - display.drawString (100, 16, "OK"); + display.drawString (90, 9, "OK"); + display.display(); + + display.drawString (0, 18, "MSP"); + display.display(); + + Serial1.begin(115200, SERIAL_8N1, SERIAL_PIN_RX , SERIAL_PIN_TX); + msp.begin(Serial1); + + display.drawString (90, 18, "OK"); display.display(); + display.drawString (0, 27, "HOST"); + display.display(); + + delay(SERIAL_FC_TIMEOUT); + msp_set_name(); + msp_set_fc(); - msp_init(); + display.drawString (90, 27, String(fc_name[curr.fc])); + display.display(); + delay(500); pinMode(interruptPin, INPUT); io_button_pressed = 0; attachInterrupt(digitalPinToInterrupt(interruptPin), handleInterrupt, RISING); - display.drawString (0, 40, "SCAN"); + display.drawString (0, 38, "SCAN"); display.display(); for (int i = 0; i < LORA_MAXPEERS; i++) { @@ -433,20 +413,19 @@ void loop() { } lora_mode = LORA_START; } else { - - sys_num_peers = 0; - for (int i = 0; i < LORA_MAXPEERS; i++) { - if (peers[i].id > 0) { - sys_num_peers++; + if ((millis() > display_updated + CYCLE_DISPLAY) && display_enabled) { + sys_num_peers = 0; + for (int i = 0; i < LORA_MAXPEERS; i++) { + if (peers[i].id > 0) { + display.drawString(40 + peers[i].id * 8, 38, String(peers[i].id)); + sys_num_peers++; + } } - } - - if (sys_num_peers > scan_last_peers) { - display.drawString (40 + sys_num_peers * 8, 40, String(sys_num_peers)); + display.drawProgressBar(00, 53, 126, 7, 100 * (millis() - cycle_scan_begin) / CYCLE_SCAN ); display.display(); - scan_last_peers = sys_num_peers; - } - + display_updated = millis(); + } + } sys_pps = 0; sys_ppsc = 0; @@ -461,11 +440,11 @@ void loop() { } else { // Not alone, sync by slot - bool gogogo = 0; - for (int i = 0; (i < LORA_MAXPEERS) && (gogogo == 0); i++) { + bool startnow = 0; + for (int i = 0; (i < LORA_MAXPEERS) && (startnow == 0); i++) { if (peers[i].id > 0) { lora_last_tx = peers[i].updated + (curr.id - peers[i].id) * LORA_CYCLE_SLOTSPACING + LORA_CYCLE_TIMING_DELAY; - gogogo = 1; + startnow = 1; } } @@ -475,6 +454,9 @@ void loop() { lora_mode = LORA_RX; sys_pps = 0; sys_ppsc = 0; + stat.packets_total = 0; + stat.packets_received = 0; + stat.percent_received = 0; } @@ -485,13 +467,12 @@ void loop() { cycle_adjust_timing = 0; } - // ---------------------- LORA TX if (lora_mode == LORA_TX) { - if (String(sys_curr_fc) == "INAV" ) { - msp_set_curr_gps(); + if (curr.fc == FC_INAV) { + msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); if (curr.gps.fixType > 0) { peerout.lat = curr.gps.lat; @@ -509,27 +490,22 @@ void loop() { } } - if (sys_tx_ticker < 255) { - sys_tx_ticker++; - } - else { - sys_tx_ticker = 0; - } +// if (curr.tick < 255) { + curr.tick++; +// } +// else { +// curr.tick = 0; +// } peerout.id = curr.id; - peerout.name[0] = curr.name[0]; - peerout.name[1] = curr.name[1]; - peerout.name[2] = curr.name[2]; - peerout.name[3] = curr.name[3]; - peerout.name[4] = curr.name[4]; - peerout.name[5] = curr.name[5]; - peerout.name[6] = curr.name[6]; - peerout.tick = sys_tx_ticker; + peerout.tick = curr.tick; + strncpy(peerout.name, curr.name, 3); last_tx_begin = millis(); lora_send(&peerout); lora_last_tx = millis(); last_tx_duration = lora_last_tx - last_tx_begin; + lora_mode = LORA_RX; if (sys_num_peers > 0) { @@ -550,7 +526,7 @@ void loop() { } } */ - if (String(sys_curr_fc) == "INAV" ) { + if (curr.fc == FC_INAV) { msp_send_peers(); } @@ -559,13 +535,11 @@ void loop() { lora_mode = LORA_RX; } - if ((millis() > display_updated + CYCLE_DISPLAY) && display_enabled && (lora_mode > LORA_START)) { display_draw(); display_updated = millis(); } - if ((millis() > (CYCLE_MAIN + main_updated)) && (lora_mode > LORA_START)) { sys_num_peers = 0; sys_pps = sys_ppsc; @@ -579,14 +553,19 @@ void loop() { if (peers[i].id > 0 && ((millis() - peers[i].updated) > LORA_PEER_TIMEOUT)) { peers[i].state = 0; peers[i].id = 0; - sys_rssi = "0"; + sys_rssi = 0; String("").toCharArray(peers[i].name, 7); } } - if (String(sys_curr_fc) == "INAV" ) { - msp_set_curr_state(); - msp_set_curr_vbat(); + stat.packets_total += sys_num_peers * CYCLE_MAIN / LORA_CYCLE; + stat.packets_received += sys_pps; + stat.percent_received = (stat.packets_received > 0) ? constrain(100 * stat.packets_received / stat.packets_total, 0 ,100) : 0; + + + if (curr.fc == FC_INAV) { + msp_set_state(); + msp.request(MSP_ANALOG, &curr.vbat, sizeof(curr.vbat)); } if ((curr.state == 0) && (display_enabled == 0)) { // Aircraft is disarmed = Turning on the OLED @@ -600,5 +579,4 @@ void loop() { main_updated = millis(); } - } diff --git a/src/main.h b/src/main.h index d7a90b9..4e1d770 100644 --- a/src/main.h +++ b/src/main.h @@ -1,4 +1,4 @@ -#define VERSION "O1" +#define VERSION "O.0.9" #define LORA_INIT 0 #define LORA_START 1 @@ -10,8 +10,8 @@ #define LORA_CODING_RATE 5 #define LORA_SPREADING_FACTOR 7 #define LORA_POWER 20 -#define LORA_CYCLE 480 // ms -#define LORA_CYCLE_SLOTSPACING 96 // ms +#define LORA_CYCLE 500 // ms +#define LORA_CYCLE_SLOTSPACING 100 // ms #define LORA_CYCLE_TIMING_DELAY -10 // ms #define LORA_CYCLE_ANTICOLLISION 30 // ms @@ -22,9 +22,10 @@ #define CYCLE_DISPLAY 200 // ms #define CYCLE_MAIN 1000 // ms -#define LORA_MAXPEERS 5 +#define LORA_MAXPEERS 8 + #define LORA_PEER_TIMEOUT 3000 // ms -#define SERIAL_FC_TIMEOUT 3000 // ms +#define SERIAL_FC_TIMEOUT 4000 // ms #define SCK 5 // GPIO5 - SX1278's SCK #define MISO 19 // GPIO19 - SX1278's MISO @@ -33,19 +34,34 @@ #define RST 14 // GPIO14 - SX1278's RESET #define DI0 26 // GPIO26 - SX1278's IRQ (interrupt request) +#define FC_NONE 0 +#define FC_INAV 1 + +char fc_name[2][5]={"None", "iNav"}; + struct peer_t { uint8_t id; uint8_t state; - char name[7]; + char name[6]; uint8_t tick; uint32_t updated; - int16_t rssi; + int rssi; + msp_raw_gps_t gps; +}; + +struct curr_t { + uint8_t id; + uint8_t state; + char name[16]; + uint8_t tick; msp_raw_gps_t gps; + msp_analog_t vbat; + uint8_t fc; }; struct peer_air_t { uint8_t id; - char name[7]; + char name[3]; uint8_t tick; int32_t lat; int32_t lon; @@ -54,3 +70,8 @@ struct peer_air_t { int16_t heading; }; +struct stats_t { + float packets_total; + uint32_t packets_received; + uint8_t percent_received; +}; From f87eff7abbbcb303a23ceab670ab44ea2a519a2d Mon Sep 17 00:00:00 2001 From: Olivier C Date: Fri, 19 Apr 2019 20:56:09 +0200 Subject: [PATCH 06/17] Drift correction --- src/main.cpp | 297 ++++++++++++++++++++++++++++++--------------------- src/main.h | 83 ++++++++------ 2 files changed, 227 insertions(+), 153 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 0647caf..d8c6ec1 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,9 +10,9 @@ // ----------------------------------------------------------------------------- global vars MSP msp; -stats_t stat; +stats_t stats; -int sys_display_page = 0; +uint8_t sys_display_page = 0; SSD1306 display(0x3c, 4, 15); curr_t curr; // Our peer @@ -20,8 +20,7 @@ peer_t peers[LORA_MAXPEERS]; // Other peers peer_air_t incoming; // Received peer peer_air_t peerout; // Sent peer - -int sys_num_peers = -1; +int sys_num_peers = 0; // String sys_rssi = "0"; int sys_rssi; @@ -30,26 +29,33 @@ uint8_t sys_pps = 0; uint8_t sys_ppsc = 0; bool io_button_pressed = 0; -long io_button_released = 0; +uint32_t io_button_released = 0; bool display_enabled = 1; -long display_updated = 0; +uint32_t display_updated = 0; -long cycle_scan_begin; +uint32_t cycle_scan_begin; uint8_t scan_last_peers = -1; -int lora_mode = LORA_INIT; +uint8_t lora_mode = LORA_INIT; + +uint8_t last_received_id = 0; + +uint32_t last_tx_begin; +uint32_t last_tx_end; -int last_received_id = 0; -int last_tx_duration = 0; -long last_tx_begin; -long last_tx_end; +uint32_t lora_origin = 0; +uint32_t lora_last_tx = 0; +uint32_t lora_next_tx = 0; +uint32_t lora_last_rx = 0; +int32_t lora_drift = 0; +uint32_t stats_updated = 0; -long lora_last_tx = 0; -long lora_last_rx = 0; -long main_updated = 0; +uint32_t msp_next_cycle = 0; +bool msp_done = 1; + +uint32_t now = 0; -long cycle_adjust_timing = 0; // -------- LoRa @@ -62,6 +68,7 @@ void lora_send(peer_air_t *outgoing) { } void onReceive(int packetSize) { + if (packetSize == 0) return; lora_last_rx = millis(); LoRa.readBytes((uint8_t *)&incoming, packetSize); @@ -69,17 +76,18 @@ void onReceive(int packetSize) { sys_ppsc++; uint8_t id = incoming.id - 1; - + // if (String(incoming.name) != String(peers[id].name)) { // Something wrong, id slot / name mismatch -// peers[incoming.id].state = 3; -// } - +// peers[incoming.id].state = 3; +// } + peers[id].id = incoming.id; last_received_id = incoming.id; strncpy(peers[id].name, incoming.name, 3); peers[id].name[3] = 0; - + + peers[id].host = incoming.host; peers[id].tick = incoming.tick; peers[id].updated = lora_last_rx; peers[id].rssi = sys_rssi; @@ -88,6 +96,9 @@ void onReceive(int packetSize) { peers[id].gps.alt = incoming.alt; peers[id].gps.groundSpeed = incoming.speed; peers[id].gps.groundCourse = incoming.heading; + + stats.timer_end = millis(); + stats.last_rx_duration = stats.timer_end - lora_last_rx; } void lora_init() { @@ -144,20 +155,19 @@ void display_enable() { void display_draw() { display.clear(); if (sys_display_page == 0) { - + display.setFont(ArialMT_Plain_24); display.setTextAlignment(TEXT_ALIGN_RIGHT); display.drawString(26, 0, String(curr.gps.numSat)); display.drawString(13, 42, String(sys_num_peers)); // display.drawString(119, 0, String((float)curr.vbat / 10)); - + display.setFont(ArialMT_Plain_10); - display.drawString (126, 23, "_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ "); + display.drawString (126, 23, "_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ "); display.drawString(107, 35, String(sys_pps)); display.drawString(65, 45, String(sys_rssi)); -// display.drawString (80, 44, String(curr.tick)); - display.drawString (107, 45, String(stat.percent_received)); - display.drawString (65, 35, String(last_tx_duration)); + display.drawString (107, 45, String(stats.percent_received)); + display.drawString (80, 35, String(peerout.tick)); display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString (60, 21, String(curr.name)); @@ -166,12 +176,12 @@ void display_draw() { display.drawString (15, 45, "ID"); display.drawString (28, 45, String(curr.id)); // display.drawString (120,12, "V"); - display.drawString (66, 35, "ms"); +// display.drawString (66, 35, "ms"); display.drawString (109, 35, "PS"); display.drawString (66, 45, "dB"); - display.drawString (0, 21, "FC:" + String(fc_name[curr.fc])); - + display.drawString (0, 21, String(host_name[curr.host])); + if (last_received_id > 0) { display.drawString (48 + last_received_id * 8, 54, String(last_received_id)); } @@ -185,9 +195,9 @@ void display_draw() { else { display.drawString (15, 54, "Peers"); } - + if (curr.gps.fixType == 1) display.drawString (33,7, "2D"); - if (curr.gps.fixType == 2) display.drawString (33,7, "3D"); + if (curr.gps.fixType == 2) display.drawString (33,7, "3D"); } else if (sys_display_page == 1) { @@ -234,20 +244,44 @@ void display_draw() { display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString (0, line, String(peers[i].id) + ":"); display.drawString (16, line, String(peers[i].name)); + display.drawString (48, line, String(host_name[peers[i].host])); display.setTextAlignment (TEXT_ALIGN_RIGHT); - if (last_tx_begin > peers[i].updated) { - display.drawString (120, line, String(last_tx_begin - peers[i].updated)); + if (lora_last_tx > peers[i].updated) { + display.drawString (120, line, String(lora_last_tx - peers[i].updated)); display.drawString (128, line, "-"); } else { - display.drawString (120, line, String((peers[i].updated) - last_tx_begin)); + display.drawString (120, line, String((peers[i].updated) - lora_last_tx)); display.drawString (128, line, "+"); } j++; } } } + else if (sys_display_page == 2) { + display.setFont (ArialMT_Plain_10); + display.setTextAlignment (TEXT_ALIGN_LEFT); + display.drawString(0, 0, "TX TIME"); + display.drawString(0, 10, "RX TIME"); + display.drawString(0, 20, "MSP TIME"); + display.drawString(0, 30, "OLED TIME"); + display.drawString(0, 40, "LORA CYCLE"); + + display.drawString(112, 0, "ms"); + display.drawString(112, 10, "ms"); + display.drawString(112, 20, "ms"); + display.drawString(112, 30, "ms"); + display.drawString(112, 40, "ms"); + + display.setTextAlignment(TEXT_ALIGN_RIGHT); + display.drawString (111, 0, String(stats.last_tx_duration)); + display.drawString (111, 10, String(stats.last_rx_duration)); + display.drawString (111, 20, String(stats.last_msp_duration)); + display.drawString (111, 30, String(stats.last_oled_duration)); + display.drawString (111, 40, String(LORA_CYCLE)); + + } last_received_id = 0; display.display(); } @@ -271,16 +305,21 @@ void msp_set_name() { } curr.name[3] = 0; } - + } void msp_set_fc() { char j[5]; - curr.fc = FC_NONE; msp.request(MSP_FC_VARIANT, &j, sizeof(j)); - + if (strcmp(j, "INAV") == 0) { - curr.fc = FC_INAV; + curr.host = HOST_INAV; + } + else if (strcmp(j, "BTFL") == 0) { + curr.host = HOST_BTFL; + } + else { + curr.host = HOST_NONE; } } @@ -301,7 +340,7 @@ void msp_send_peers() { } } -// ----------------------------------------------------------------------------- main init +// -------- INTERRUPTS const byte interruptPin = 0; volatile int interruptCounter = 0; @@ -315,7 +354,7 @@ void IRAM_ATTR handleInterrupt() { if (io_button_pressed == 0) { io_button_pressed = 1; - if (sys_display_page >= 1) { + if (sys_display_page >= 2) { sys_display_page = 0; } else { @@ -327,6 +366,20 @@ void IRAM_ATTR handleInterrupt() { portEXIT_CRITICAL_ISR(&mux); } +// -------- SYSTEM + +int count_peers() { + int j = 0; + for (int i = 0; i < LORA_MAXPEERS; i++) { + if (peers[i].id > 0) { + j++; + } + } + return j; +} + +// ----------------------------- + void setup() { display_init(); @@ -354,9 +407,9 @@ void setup() { msp_set_name(); msp_set_fc(); - display.drawString (90, 27, String(fc_name[curr.fc])); + display.drawString (90, 27, String(host_name[curr.host])); display.display(); - + delay(500); pinMode(interruptPin, INPUT); @@ -371,10 +424,11 @@ void setup() { } curr.id = 0; + strncpy(peerout.name, curr.name, 3); + peerout.host = curr.host; lora_last_tx = millis(); - main_updated = millis(); - + stats_updated = millis(); display_updated = millis(); cycle_scan_begin = millis(); @@ -382,7 +436,6 @@ void setup() { LoRa.receive(); lora_mode = LORA_INIT; - } // ----------------------------------------------------------------------------- MAIN LOOP @@ -406,9 +459,8 @@ void loop() { if ((peers[i].id == 0)) { if (curr.id == 0) { curr.id = i + 1; + peerout.id = curr.id; } - } else { - sys_num_peers++; } } lora_mode = LORA_START; @@ -417,62 +469,68 @@ void loop() { sys_num_peers = 0; for (int i = 0; i < LORA_MAXPEERS; i++) { if (peers[i].id > 0) { - display.drawString(40 + peers[i].id * 8, 38, String(peers[i].id)); - sys_num_peers++; + display.drawString(40 + peers[i].id * 8, 38, String(peers[i].id)); } } - display.drawProgressBar(00, 53, 126, 7, 100 * (millis() - cycle_scan_begin) / CYCLE_SCAN ); + display.drawProgressBar(00, 53, 126, 7, 100 * (millis() - cycle_scan_begin) / CYCLE_SCAN ); display.display(); display_updated = millis(); - } - + } } - sys_pps = 0; - sys_ppsc = 0; } // ---------------------- LORA START if (lora_mode == LORA_START) { + sys_num_peers = count_peers(); + + lora_origin = millis(); + if (sys_num_peers == 0) { // Alone, start at will - lora_last_tx = millis(); - } + lora_next_tx = lora_origin + LORA_CYCLE + LORA_CYCLE_TIMING_DELAY; + + } else { // Not alone, sync by slot bool startnow = 0; for (int i = 0; (i < LORA_MAXPEERS) && (startnow == 0); i++) { if (peers[i].id > 0) { - lora_last_tx = peers[i].updated + (curr.id - peers[i].id) * LORA_CYCLE_SLOTSPACING + LORA_CYCLE_TIMING_DELAY; - startnow = 1; + lora_next_tx = peers[i].updated + (curr.id - peers[i].id) * LORA_CYCLE_SLOTSPACING + LORA_CYCLE * 2 + LORA_CYCLE_TIMING_DELAY; + startnow = 1; } } } display_updated = millis(); - main_updated = millis(); - lora_mode = LORA_RX; + stats_updated = millis(); sys_pps = 0; sys_ppsc = 0; - stat.packets_total = 0; - stat.packets_received = 0; - stat.percent_received = 0; + sys_num_peers = 0; + stats.packets_total = 0; + stats.packets_received = 0; + stats.percent_received = 0; + peerout.tick = 0; + lora_mode = LORA_RX; } // ---------------------- LORA RX - if ((lora_mode == LORA_RX) && (millis() > (lora_last_tx + LORA_CYCLE + cycle_adjust_timing))) { - lora_mode = LORA_TX; - cycle_adjust_timing = 0; + if ((lora_mode == LORA_RX) && (millis() > lora_next_tx)) { + now = millis(); + + while (now > lora_next_tx) { // In case we skipped soome beats + lora_next_tx += LORA_CYCLE; } + lora_mode = LORA_TX; + } // ---------------------- LORA TX if (lora_mode == LORA_TX) { - if (curr.fc == FC_INAV) { - msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); + if (curr.host != HOST_NONE) { if (curr.gps.fixType > 0) { peerout.lat = curr.gps.lat; @@ -490,83 +548,82 @@ void loop() { } } -// if (curr.tick < 255) { - curr.tick++; -// } -// else { -// curr.tick = 0; -// } + peerout.tick++; - peerout.id = curr.id; - peerout.tick = curr.tick; - strncpy(peerout.name, curr.name, 3); - - last_tx_begin = millis(); + stats.last_tx_begin = millis(); lora_send(&peerout); lora_last_tx = millis(); - last_tx_duration = lora_last_tx - last_tx_begin; + stats.last_tx_duration = lora_last_tx - stats.last_tx_begin; - lora_mode = LORA_RX; + msp_done = 0; + msp_next_cycle = lora_last_tx + MSP_CYCLE_DELAY; - if (sys_num_peers > 0) { - for (int i = 0; i < LORA_MAXPEERS; i++) { - if (peers[i].id > 0) { - if (lora_last_tx - peers[i].updated < LORA_CYCLE_ANTICOLLISION) { - cycle_adjust_timing = LORA_CYCLE_ANTICOLLISION; - } - } - } - } - - /* - if (curr.id > 1) { - int prev = curr.id - 1; - if ((peers[prev].id > 1) && (peers[prev].updated > 0) && ((lora_last_tx - peers[prev].updated) < (LORA_CYCLE_SLOTSPACING / 2))) { - cycle_adjust_timing = LORA_CYCLE_SLOTSPACING / 2; + if (peers[curr.id].id > 1 && peers[curr.id - 1].id > 0) { + lora_drift = lora_last_tx - peers[curr.id - 1].updated - LORA_CYCLE_SLOTSPACING; + + if ((abs(lora_drift) > LORA_CYCLE_ANTIDRIFT_THRESHOLD) && (abs(lora_drift) < LORA_CYCLE)) { + lora_next_tx += constrain(lora_drift, -LORA_CYCLE_ANTIDRIFT_CORRECTION, LORA_CYCLE_ANTIDRIFT_CORRECTION); } } -*/ - if (curr.fc == FC_INAV) { - msp_send_peers(); - } LoRa.sleep(); LoRa.receive(); lora_mode = LORA_RX; } +// ---------------------- DISPLAY + if ((millis() > display_updated + CYCLE_DISPLAY) && display_enabled && (lora_mode > LORA_START)) { + + stats.timer_begin = millis(); display_draw(); + stats.timer_end = millis(); + stats.last_oled_duration = stats.timer_end - stats.timer_begin; + display_updated = millis(); } - if ((millis() > (CYCLE_MAIN + main_updated)) && (lora_mode > LORA_START)) { - sys_num_peers = 0; +// ---------------------- SERIAL / MSP + + if ((millis() > msp_next_cycle && !msp_done && (curr.host != HOST_NONE)) && (lora_mode > LORA_START)) { + stats.timer_begin = millis(); + + msp_send_peers(); + msp_set_state(); + msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); + + stats.timer_end = millis(); + stats.last_msp_duration = stats.timer_end - stats.timer_begin; + stats.last_msp_delay = stats.timer_begin - lora_last_tx; + +// if (display_enabled == 1) { +// msp.request(MSP_ANALOG, &curr.vbat, sizeof(curr.vbat)); +// } + + msp_done = 1; + } + +// ---------------------- STATISTICS & IO + + if ((millis() > (CYCLE_STATS + stats_updated)) && (lora_mode > LORA_START)) { + sys_pps = sys_ppsc; sys_ppsc = 0; - + now = millis(); + for (int i = 0; i < LORA_MAXPEERS; i++) { - if (peers[i].id > 0) { - sys_num_peers++; - } - - if (peers[i].id > 0 && ((millis() - peers[i].updated) > LORA_PEER_TIMEOUT)) { + if (peers[i].id > 0 && ((now - peers[i].updated) > LORA_PEER_TIMEOUT)) { peers[i].state = 0; peers[i].id = 0; sys_rssi = 0; - String("").toCharArray(peers[i].name, 7); } } - - stat.packets_total += sys_num_peers * CYCLE_MAIN / LORA_CYCLE; - stat.packets_received += sys_pps; - stat.percent_received = (stat.packets_received > 0) ? constrain(100 * stat.packets_received / stat.packets_total, 0 ,100) : 0; - - if (curr.fc == FC_INAV) { - msp_set_state(); - msp.request(MSP_ANALOG, &curr.vbat, sizeof(curr.vbat)); - } + sys_num_peers = count_peers(); + + stats.packets_total += sys_num_peers * CYCLE_STATS / LORA_CYCLE; + stats.packets_received += sys_pps; + stats.percent_received = (stats.packets_received > 0) ? constrain(100 * stats.packets_received / stats.packets_total, 0 ,100) : 0; if ((curr.state == 0) && (display_enabled == 0)) { // Aircraft is disarmed = Turning on the OLED display_enable(); @@ -576,7 +633,7 @@ void loop() { display_disable(); } - main_updated = millis(); + stats_updated = millis(); } } diff --git a/src/main.h b/src/main.h index 4e1d770..9db65a0 100644 --- a/src/main.h +++ b/src/main.h @@ -12,17 +12,19 @@ #define LORA_POWER 20 #define LORA_CYCLE 500 // ms #define LORA_CYCLE_SLOTSPACING 100 // ms -#define LORA_CYCLE_TIMING_DELAY -10 // ms -#define LORA_CYCLE_ANTICOLLISION 30 // ms +#define LORA_CYCLE_TIMING_DELAY -16 // ms +#define LORA_CYCLE_ANTIDRIFT_THRESHOLD 5 // ms +#define LORA_CYCLE_ANTIDRIFT_CORRECTION 5 // ms +#define MSP_CYCLE_DELAY 250 // ms #define SERIAL_PIN_TX 23 #define SERIAL_PIN_RX 17 #define CYCLE_SCAN 4000 // ms -#define CYCLE_DISPLAY 200 // ms -#define CYCLE_MAIN 1000 // ms +#define CYCLE_DISPLAY 125 // ms +#define CYCLE_STATS 1000 // ms -#define LORA_MAXPEERS 8 +#define LORA_MAXPEERS 4 #define LORA_PEER_TIMEOUT 3000 // ms #define SERIAL_FC_TIMEOUT 4000 // ms @@ -34,44 +36,59 @@ #define RST 14 // GPIO14 - SX1278's RESET #define DI0 26 // GPIO26 - SX1278's IRQ (interrupt request) -#define FC_NONE 0 -#define FC_INAV 1 +#define HOST_NONE 0 +#define HOST_INAV 1 +#define HOST_BTFL 2 -char fc_name[2][5]={"None", "iNav"}; +char host_name[3][5]={"NoFC", "iNav", "Beta"}; struct peer_t { - uint8_t id; - uint8_t state; - char name[6]; - uint8_t tick; - uint32_t updated; - int rssi; - msp_raw_gps_t gps; + uint8_t id; + uint8_t state; + char name[6]; + uint8_t host; + uint8_t tick; + uint32_t updated; + int rssi; + msp_raw_gps_t gps; }; struct curr_t { - uint8_t id; - uint8_t state; - char name[16]; - uint8_t tick; - msp_raw_gps_t gps; - msp_analog_t vbat; - uint8_t fc; + uint8_t id; + uint8_t state; + char name[16]; + uint8_t tick; + msp_raw_gps_t gps; + msp_analog_t vbat; + uint8_t host; }; struct peer_air_t { - uint8_t id; - char name[3]; - uint8_t tick; - int32_t lat; - int32_t lon; - int16_t alt; - int16_t speed; - int16_t heading; + uint8_t id; + char name[3]; + uint8_t host; + uint8_t tick; + int32_t lat; + int32_t lon; + int16_t alt; + int16_t speed; + int16_t heading; }; struct stats_t { - float packets_total; - uint32_t packets_received; - uint8_t percent_received; + uint32_t timer_begin; + uint32_t timer_end; + float packets_total; + uint32_t packets_received; + uint8_t percent_received; + uint16_t last_tx_begin; + uint16_t last_tx_duration; + uint16_t last_rx_duration; + uint16_t last_msp_duration; + uint16_t last_oled_duration; + uint16_t last_msp_delay; }; + + + + From 788e3d31d1aa80537bd898de431fa2342626565b Mon Sep 17 00:00:00 2001 From: Olivier C Date: Fri, 19 Apr 2019 23:35:57 +0200 Subject: [PATCH 07/17] Major cleaning --- src/main.cpp | 146 +++++++++++++++++++++++++-------------------------- src/main.h | 7 ++- 2 files changed, 74 insertions(+), 79 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index d8c6ec1..f8619da 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -9,10 +9,6 @@ // ----------------------------------------------------------------------------- global vars -MSP msp; -stats_t stats; - -uint8_t sys_display_page = 0; SSD1306 display(0x3c, 4, 15); curr_t curr; // Our peer @@ -20,40 +16,41 @@ peer_t peers[LORA_MAXPEERS]; // Other peers peer_air_t incoming; // Received peer peer_air_t peerout; // Sent peer -int sys_num_peers = 0; - -// String sys_rssi = "0"; +MSP msp; +stats_t stats; int sys_rssi; +uint8_t lora_mode; +uint8_t last_received_id = 0; +char sys_message[20]; + +// --- Counters uint8_t sys_pps = 0; uint8_t sys_ppsc = 0; +int sys_num_peers = 0; -bool io_button_pressed = 0; -uint32_t io_button_released = 0; +// --- Inputs outputs +uint8_t sys_display_page = 0; +uint32_t io_button_released = 0; +bool io_button_pressed = 0; bool display_enabled = 1; -uint32_t display_updated = 0; - -uint32_t cycle_scan_begin; -uint8_t scan_last_peers = -1; -uint8_t lora_mode = LORA_INIT; -uint8_t last_received_id = 0; +// Timing +uint32_t cycle_scan_begin; uint32_t last_tx_begin; uint32_t last_tx_end; - uint32_t lora_origin = 0; uint32_t lora_last_tx = 0; uint32_t lora_next_tx = 0; uint32_t lora_last_rx = 0; int32_t lora_drift = 0; uint32_t stats_updated = 0; - uint32_t msp_next_cycle = 0; bool msp_done = 1; - +uint32_t display_updated = 0; uint32_t now = 0; @@ -83,10 +80,8 @@ void onReceive(int packetSize) { peers[id].id = incoming.id; last_received_id = incoming.id; - strncpy(peers[id].name, incoming.name, 3); peers[id].name[3] = 0; - peers[id].host = incoming.host; peers[id].tick = incoming.tick; peers[id].updated = lora_last_rx; @@ -102,8 +97,6 @@ void onReceive(int packetSize) { } void lora_init() { - display.drawString (0, 9, "LORA"); - display.display(); SPI.begin(5, 19, 27, 18); LoRa.setPins(SS, RST, DI0); @@ -175,19 +168,17 @@ void display_draw() { display.drawString (108, 45, "%E"); display.drawString (15, 45, "ID"); display.drawString (28, 45, String(curr.id)); -// display.drawString (120,12, "V"); -// display.drawString (66, 35, "ms"); display.drawString (109, 35, "PS"); display.drawString (66, 45, "dB"); - display.drawString (0, 21, String(host_name[curr.host])); +// display.drawString (120,12, "V"); if (last_received_id > 0) { display.drawString (48 + last_received_id * 8, 54, String(last_received_id)); } if (sys_num_peers == 0) { - display.drawString (15, 54, "Alone"); + display.drawString (15, 54, "Solo"); } else if (sys_num_peers == 1) { display.drawString (15, 54, "Peer"); @@ -201,6 +192,7 @@ void display_draw() { } else if (sys_display_page == 1) { + display.setFont (ArialMT_Plain_10); display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString (0, 1 , "_____________________"); @@ -213,15 +205,15 @@ void display_draw() { int j = 0; long pos[LORA_MAXPEERS]; - long tl_begin = millis(); + now = millis(); long diff; int line; for (int i = 0; i < LORA_MAXPEERS ; i++) { if (peers[i].id != 0) { - diff = tl_begin - peers[i].updated; - if ( diff < 480) { - pos[i] = 120 - diff / 4; + diff = now - peers[i].updated; + if ( diff < LORA_CYCLE) { + pos[i] = 120 - 120 * diff / LORA_CYCLE; } } else { @@ -229,9 +221,9 @@ void display_draw() { } } - diff = tl_begin - lora_last_tx; - if ( diff < 480) { - display.drawString (120 - diff / 4, 0, "0"); + diff = now - lora_last_tx; + if ( diff < LORA_CYCLE) { + display.drawString (120 - 120 * diff / LORA_CYCLE, 0, "0"); } for (int i = 0; i < LORA_MAXPEERS ; i++) { @@ -259,7 +251,9 @@ void display_draw() { } } } + else if (sys_display_page == 2) { + display.setFont (ArialMT_Plain_10); display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString(0, 0, "TX TIME"); @@ -282,7 +276,19 @@ void display_draw() { display.drawString (111, 40, String(LORA_CYCLE)); } + + else if (sys_display_page == 3) { + display.setFont (ArialMT_Plain_10); + display.setTextAlignment (TEXT_ALIGN_LEFT); + display.drawString(0, 0, "GPS LAT"); + display.drawString(0, 10, "GPS LON"); + + display.setTextAlignment(TEXT_ALIGN_RIGHT); + display.drawString (120, 0, String((float)curr.gps.lat/10000000,6)); + display.drawString (120, 10, String((float)curr.gps.lon/10000000,6)); + } last_received_id = 0; + sys_message[0] = 0; display.display(); } @@ -323,7 +329,6 @@ void msp_set_fc() { } } - void msp_send_peers() { msp_radar_pos_t radarPos; for (int i = 0; i < LORA_MAXPEERS; i++) { @@ -354,7 +359,7 @@ void IRAM_ATTR handleInterrupt() { if (io_button_pressed == 0) { io_button_pressed = 1; - if (sys_display_page >= 2) { + if (sys_display_page >= 3) { sys_display_page = 0; } else { @@ -383,35 +388,32 @@ int count_peers() { void setup() { display_init(); + display.drawString(0, 0, "RADAR VERSION"); display.drawString(90, 0, VERSION); display.display(); + display.drawString (0, 9, "LORA"); + display.display(); lora_init(); - display.drawString (90, 9, "OK"); display.display(); display.drawString (0, 18, "MSP"); display.display(); - Serial1.begin(115200, SERIAL_8N1, SERIAL_PIN_RX , SERIAL_PIN_TX); msp.begin(Serial1); - display.drawString (90, 18, "OK"); display.display(); + display.drawString (0, 27, "HOST"); display.display(); - delay(SERIAL_FC_TIMEOUT); msp_set_name(); msp_set_fc(); - display.drawString (90, 27, String(host_name[curr.host])); display.display(); - delay(500); - pinMode(interruptPin, INPUT); io_button_pressed = 0; attachInterrupt(digitalPinToInterrupt(interruptPin), handleInterrupt, RISING); @@ -427,14 +429,12 @@ void setup() { strncpy(peerout.name, curr.name, 3); peerout.host = curr.host; - lora_last_tx = millis(); - stats_updated = millis(); - display_updated = millis(); - cycle_scan_begin = millis(); - LoRa.sleep(); LoRa.receive(); + display_updated = 0; + cycle_scan_begin = millis(); + lora_mode = LORA_INIT; } @@ -452,9 +452,7 @@ void loop() { // ---------------------- LORA INIT if (lora_mode == LORA_INIT) { - - if (millis() > (cycle_scan_begin + CYCLE_SCAN)) { - + if (millis() > (cycle_scan_begin + CYCLE_SCAN)) { // End of the scan, set the ID then sync for (int i = 0; i < LORA_MAXPEERS; i++) { if ((peers[i].id == 0)) { if (curr.id == 0) { @@ -463,16 +461,15 @@ void loop() { } } } - lora_mode = LORA_START; - } else { + lora_mode = LORA_SYNC; + } else { // Still scanning if ((millis() > display_updated + CYCLE_DISPLAY) && display_enabled) { - sys_num_peers = 0; for (int i = 0; i < LORA_MAXPEERS; i++) { if (peers[i].id > 0) { display.drawString(40 + peers[i].id * 8, 38, String(peers[i].id)); } } - display.drawProgressBar(00, 53, 126, 7, 100 * (millis() - cycle_scan_begin) / CYCLE_SCAN ); + display.drawProgressBar(00, 53, 126, 7, 100 * (millis() - cycle_scan_begin) / CYCLE_SCAN); display.display(); display_updated = millis(); } @@ -481,22 +478,20 @@ void loop() { // ---------------------- LORA START - if (lora_mode == LORA_START) { + if (lora_mode == LORA_SYNC) { sys_num_peers = count_peers(); - lora_origin = millis(); if (sys_num_peers == 0) { // Alone, start at will - lora_next_tx = lora_origin + LORA_CYCLE + LORA_CYCLE_TIMING_DELAY; - + lora_next_tx = lora_origin + LORA_CYCLE; } else { // Not alone, sync by slot bool startnow = 0; for (int i = 0; (i < LORA_MAXPEERS) && (startnow == 0); i++) { if (peers[i].id > 0) { - lora_next_tx = peers[i].updated + (curr.id - peers[i].id) * LORA_CYCLE_SLOTSPACING + LORA_CYCLE * 2 + LORA_CYCLE_TIMING_DELAY; + lora_next_tx = peers[i].updated + (curr.id - peers[i].id) * LORA_CYCLE_SLOTSPACING + LORA_CYCLE + LORA_CYCLE_TIMING_DELAY; startnow = 1; } } @@ -514,13 +509,12 @@ void loop() { lora_mode = LORA_RX; } - // ---------------------- LORA RX if ((lora_mode == LORA_RX) && (millis() > lora_next_tx)) { now = millis(); - while (now > lora_next_tx) { // In case we skipped soome beats + while (now > lora_next_tx) { // In case we skipped some beats lora_next_tx += LORA_CYCLE; } lora_mode = LORA_TX; @@ -549,7 +543,6 @@ void loop() { } peerout.tick++; - stats.last_tx_begin = millis(); lora_send(&peerout); lora_last_tx = millis(); @@ -558,14 +551,18 @@ void loop() { msp_done = 0; msp_next_cycle = lora_last_tx + MSP_CYCLE_DELAY; + // Drift correction + if (peers[curr.id].id > 1 && peers[curr.id - 1].id > 0) { lora_drift = lora_last_tx - peers[curr.id - 1].updated - LORA_CYCLE_SLOTSPACING; - if ((abs(lora_drift) > LORA_CYCLE_ANTIDRIFT_THRESHOLD) && (abs(lora_drift) < LORA_CYCLE)) { + if ((abs(lora_drift) > LORA_CYCLE_ANTIDRIFT_THRESHOLD) && (abs(lora_drift) < LORA_CYCLE_SLOTSPACING * 0.5)) { lora_next_tx += constrain(lora_drift, -LORA_CYCLE_ANTIDRIFT_CORRECTION, LORA_CYCLE_ANTIDRIFT_CORRECTION); } } + // Back to RX + LoRa.sleep(); LoRa.receive(); lora_mode = LORA_RX; @@ -573,44 +570,42 @@ void loop() { // ---------------------- DISPLAY - if ((millis() > display_updated + CYCLE_DISPLAY) && display_enabled && (lora_mode > LORA_START)) { + if ((millis() > display_updated + CYCLE_DISPLAY) && display_enabled && (lora_mode > LORA_SYNC)) { stats.timer_begin = millis(); display_draw(); stats.timer_end = millis(); stats.last_oled_duration = stats.timer_end - stats.timer_begin; - display_updated = millis(); } // ---------------------- SERIAL / MSP - if ((millis() > msp_next_cycle && !msp_done && (curr.host != HOST_NONE)) && (lora_mode > LORA_START)) { + if ((millis() > msp_next_cycle && !msp_done && (curr.host != HOST_NONE)) && (lora_mode > LORA_SYNC)) { stats.timer_begin = millis(); - msp_send_peers(); msp_set_state(); msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); - stats.timer_end = millis(); - stats.last_msp_duration = stats.timer_end - stats.timer_begin; - stats.last_msp_delay = stats.timer_begin - lora_last_tx; - // if (display_enabled == 1) { // msp.request(MSP_ANALOG, &curr.vbat, sizeof(curr.vbat)); // } + stats.timer_end = millis(); + stats.last_msp_duration = stats.timer_end - stats.timer_begin; msp_done = 1; } // ---------------------- STATISTICS & IO - if ((millis() > (CYCLE_STATS + stats_updated)) && (lora_mode > LORA_START)) { + if ((millis() > (CYCLE_STATS + stats_updated)) && (lora_mode > LORA_SYNC)) { sys_pps = sys_ppsc; sys_ppsc = 0; now = millis(); + // Pruning the timed-out peers + for (int i = 0; i < LORA_MAXPEERS; i++) { if (peers[i].id > 0 && ((now - peers[i].updated) > LORA_PEER_TIMEOUT)) { peers[i].state = 0; @@ -618,13 +613,14 @@ void loop() { sys_rssi = 0; } } - + sys_num_peers = count_peers(); - stats.packets_total += sys_num_peers * CYCLE_STATS / LORA_CYCLE; stats.packets_received += sys_pps; stats.percent_received = (stats.packets_received > 0) ? constrain(100 * stats.packets_received / stats.packets_total, 0 ,100) : 0; - + + // Screen management + if ((curr.state == 0) && (display_enabled == 0)) { // Aircraft is disarmed = Turning on the OLED display_enable(); } diff --git a/src/main.h b/src/main.h index 9db65a0..49cac6e 100644 --- a/src/main.h +++ b/src/main.h @@ -1,7 +1,7 @@ #define VERSION "O.0.9" #define LORA_INIT 0 -#define LORA_START 1 +#define LORA_SYNC 1 #define LORA_RX 2 #define LORA_TX 3 @@ -15,7 +15,7 @@ #define LORA_CYCLE_TIMING_DELAY -16 // ms #define LORA_CYCLE_ANTIDRIFT_THRESHOLD 5 // ms #define LORA_CYCLE_ANTIDRIFT_CORRECTION 5 // ms -#define MSP_CYCLE_DELAY 250 // ms +#define MSP_CYCLE_DELAY 50 // ms #define SERIAL_PIN_TX 23 #define SERIAL_PIN_RX 17 @@ -24,7 +24,7 @@ #define CYCLE_DISPLAY 125 // ms #define CYCLE_STATS 1000 // ms -#define LORA_MAXPEERS 4 +#define LORA_MAXPEERS 5 #define LORA_PEER_TIMEOUT 3000 // ms #define SERIAL_FC_TIMEOUT 4000 // ms @@ -86,7 +86,6 @@ struct stats_t { uint16_t last_rx_duration; uint16_t last_msp_duration; uint16_t last_oled_duration; - uint16_t last_msp_delay; }; From 270d88bd6383a3644ae9d4bcf8e270f419248520 Mon Sep 17 00:00:00 2001 From: Olivier C Date: Sat, 20 Apr 2019 21:51:06 +0200 Subject: [PATCH 08/17] More tweaks --- src/lib/MSP.h | 2 +- src/main.cpp | 175 ++++++++++++++++++++++++++++++++++---------------- src/main.h | 22 ++++--- 3 files changed, 133 insertions(+), 66 deletions(-) diff --git a/src/lib/MSP.h b/src/lib/MSP.h index 659735e..c9eaf50 100644 --- a/src/lib/MSP.h +++ b/src/lib/MSP.h @@ -654,7 +654,7 @@ struct msp_radar_pos_t { int32_t alt; // cm uint16_t heading; // deg uint16_t speed; // cm/s - uint8_t lq; // rssi + uint8_t tick; // rssi } __attribute__((packed)); struct msp_radar_itd_t { diff --git a/src/main.cpp b/src/main.cpp index f8619da..2daaa65 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -49,7 +49,7 @@ uint32_t lora_last_rx = 0; int32_t lora_drift = 0; uint32_t stats_updated = 0; uint32_t msp_next_cycle = 0; -bool msp_done = 1; +int msp_step = 0; uint32_t display_updated = 0; uint32_t now = 0; @@ -80,17 +80,21 @@ void onReceive(int packetSize) { peers[id].id = incoming.id; last_received_id = incoming.id; - strncpy(peers[id].name, incoming.name, 3); - peers[id].name[3] = 0; + peers[id].host = incoming.host; - peers[id].tick = incoming.tick; peers[id].updated = lora_last_rx; peers[id].rssi = sys_rssi; + peers[id].gps.lat = incoming.lat; peers[id].gps.lon = incoming.lon; peers[id].gps.alt = incoming.alt; - peers[id].gps.groundSpeed = incoming.speed; peers[id].gps.groundCourse = incoming.heading; + peers[id].gps.groundSpeed = incoming.speed; + + peers[id].tick = incoming.tick; + + strncpy(peers[id].name, incoming.name, LORA_NAME_LENGTH); + // peers[id].name[LORA_NAME_LENGTH - 1] = 0; stats.timer_end = millis(); stats.last_rx_duration = stats.timer_end - lora_last_rx; @@ -132,21 +136,21 @@ void display_init() { } void display_disable() { -// display.clear(); -// display.display(); display.displayOff(); display_enabled = 0; } void display_enable() { -// display.clear(); -// display.display(); display.displayOn(); display_enabled = 1; } void display_draw() { display.clear(); + + int j = 0; + int line; + if (sys_display_page == 0) { display.setFont(ArialMT_Plain_24); @@ -187,12 +191,12 @@ void display_draw() { display.drawString (15, 54, "Peers"); } - if (curr.gps.fixType == 1) display.drawString (33,7, "2D"); - if (curr.gps.fixType == 2) display.drawString (33,7, "3D"); + if (curr.gps.fixType == 1) display.drawString (30,3, "2D"); + if (curr.gps.fixType == 2) display.drawString (30,3, "3D"); } else if (sys_display_page == 1) { - + display.setFont (ArialMT_Plain_10); display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString (0, 1 , "_____________________"); @@ -203,11 +207,9 @@ void display_draw() { display.drawString (52, 14 , String(LORA_CYCLE / 2)); display.drawString (120, 14 , "0"); - int j = 0; long pos[LORA_MAXPEERS]; now = millis(); long diff; - int line; for (int i = 0; i < LORA_MAXPEERS ; i++) { if (peers[i].id != 0) { @@ -223,7 +225,7 @@ void display_draw() { diff = now - lora_last_tx; if ( diff < LORA_CYCLE) { - display.drawString (120 - 120 * diff / LORA_CYCLE, 0, "0"); + display.drawString (120 - 120 * diff / LORA_CYCLE, 0, String(curr.id)); } for (int i = 0; i < LORA_MAXPEERS ; i++) { @@ -231,7 +233,7 @@ void display_draw() { display.drawString (pos[i], 0, String(peers[i].id)); } - if (peers[i].id != 0 && j < 4) { + if (peers[i].id > 0 && j < 4) { line = j * 9 + 24; display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString (0, line, String(peers[i].id) + ":"); @@ -251,9 +253,9 @@ void display_draw() { } } } - + else if (sys_display_page == 2) { - + display.setFont (ArialMT_Plain_10); display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString(0, 0, "TX TIME"); @@ -270,24 +272,56 @@ void display_draw() { display.setTextAlignment(TEXT_ALIGN_RIGHT); display.drawString (111, 0, String(stats.last_tx_duration)); - display.drawString (111, 10, String(stats.last_rx_duration)); + // display.drawString (111, 10, String(stats.last_rx_duration)); + display.drawString (111, 10, "--"); display.drawString (111, 20, String(stats.last_msp_duration)); display.drawString (111, 30, String(stats.last_oled_duration)); display.drawString (111, 40, String(LORA_CYCLE)); } - + else if (sys_display_page == 3) { display.setFont (ArialMT_Plain_10); display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString(0, 0, "GPS LAT"); - display.drawString(0, 10, "GPS LON"); + + + display.drawString(0, 0, "HOST:"); + display.drawString(0, 10, "GPS LAT"); + display.drawString(0, 20, "GPS LON"); + display.drawString(0, 30, "GPS ALT"); + display.drawString(0, 40, "TICK"); + + display.drawString (32, 0, String(curr.name)); + display.drawString (96, 0, String(host_name[curr.host])); display.setTextAlignment(TEXT_ALIGN_RIGHT); - display.drawString (120, 0, String((float)curr.gps.lat/10000000,6)); - display.drawString (120, 10, String((float)curr.gps.lon/10000000,6)); + display.drawString (120, 10, String((float)curr.gps.lat/10000000,6)); + display.drawString (120, 20, String((float)curr.gps.lon/10000000,6)); + display.drawString (120, 30, String((float)curr.gps.alt)); + display.drawString (120, 40, String(peerout.tick)); } - last_received_id = 0; + else if (sys_display_page == 4) { + display.setFont (ArialMT_Plain_10); + display.setTextAlignment (TEXT_ALIGN_LEFT); + + int j = 0; + for (int i = 0; i < LORA_MAXPEERS ; i++) { + if (peers[i].id > 0 && j < 3) { + line = j * 20; + display.setTextAlignment (TEXT_ALIGN_LEFT); + display.drawString (0, line, String(peers[i].id) + ":"); + display.drawString (13, line, String(peers[i].name)); + display.drawString (13, line + 10, "LQ" + String(peers[i].rssi)); + display.drawString (61, line + 10, String(peers[i].tick)); + + display.setTextAlignment (TEXT_ALIGN_RIGHT); + display.drawString (128, line, String((float)peers[i].gps.lat/10000000,6)); + display.drawString (128, line + 10, String((float)peers[i].gps.lon/10000000,6)); + j++; + } + } + } + last_received_id = 0; sys_message[0] = 0; display.display(); } @@ -306,10 +340,10 @@ void msp_set_name() { // } else { - for (int i = 0; i < 3; i++) { + for (int i = 0; i < LORA_NAME_LENGTH - 1; i++) { curr.name[i] = (char) random(65, 90); } - curr.name[3] = 0; + curr.name[LORA_NAME_LENGTH - 1] = 0; } } @@ -338,8 +372,9 @@ void msp_send_peers() { radarPos.lat = peers[i].gps.lat; radarPos.lon = peers[i].gps.lon; radarPos.alt = peers[i].gps.alt; - radarPos.speed = peers[i].gps.groundSpeed; - radarPos.heading = peers[i].gps.groundCourse / 10; + radarPos.heading = peers[i].gps.groundSpeed; + radarPos.speed = peers[i].gps.groundCourse; + radarPos.tick = peers[i].tick; msp.command(MSP_SET_RADAR_POS, &radarPos, sizeof(radarPos)); } } @@ -359,7 +394,7 @@ void IRAM_ATTR handleInterrupt() { if (io_button_pressed == 0) { io_button_pressed = 1; - if (sys_display_page >= 3) { + if (sys_display_page >= 4) { sys_display_page = 0; } else { @@ -388,7 +423,7 @@ int count_peers() { void setup() { display_init(); - + display.drawString(0, 0, "RADAR VERSION"); display.drawString(90, 0, VERSION); display.display(); @@ -426,7 +461,7 @@ void setup() { } curr.id = 0; - strncpy(peerout.name, curr.name, 3); + strncpy(peerout.name, curr.name, LORA_NAME_LENGTH); peerout.host = curr.host; LoRa.sleep(); @@ -530,39 +565,51 @@ void loop() { peerout.lat = curr.gps.lat; peerout.lon = curr.gps.lon; peerout.alt = curr.gps.alt; - peerout.heading = curr.gps.groundCourse; peerout.speed = curr.gps.groundSpeed; + peerout.heading = curr.gps.groundCourse; } else { - peerout.lat = 0; - peerout.lon = 0; - peerout.alt = 0; - peerout.heading = 0; + peerout.lat = 128; + peerout.lon = 222222222; + peerout.alt = 333; peerout.speed = 0; + peerout.heading = 0; } } peerout.tick++; + stats.last_tx_begin = millis(); lora_send(&peerout); lora_last_tx = millis(); stats.last_tx_duration = lora_last_tx - stats.last_tx_begin; - msp_done = 0; - msp_next_cycle = lora_last_tx + MSP_CYCLE_DELAY; +// msp_step = 0; +// stats.last_msp_duration = 0; + //msp_next_cycle = lora_last_tx + LORA_CYCLE_SLOTSPACING / 2; // Begin sending MSP half a slot later // Drift correction - + if (peers[curr.id].id > 1 && peers[curr.id - 1].id > 0) { lora_drift = lora_last_tx - peers[curr.id - 1].updated - LORA_CYCLE_SLOTSPACING; - + if ((abs(lora_drift) > LORA_CYCLE_ANTIDRIFT_THRESHOLD) && (abs(lora_drift) < LORA_CYCLE_SLOTSPACING * 0.5)) { lora_next_tx += constrain(lora_drift, -LORA_CYCLE_ANTIDRIFT_CORRECTION, LORA_CYCLE_ANTIDRIFT_CORRECTION); } } + + if (curr.host != HOST_NONE) { + stats.timer_begin = millis(); + msp_send_peers(); + msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); + msp_set_state(); + stats.timer_end = millis(); + stats.last_msp_duration = stats.timer_end - stats.timer_begin; + } + // Back to RX - + LoRa.sleep(); LoRa.receive(); lora_mode = LORA_RX; @@ -581,20 +628,38 @@ void loop() { // ---------------------- SERIAL / MSP - if ((millis() > msp_next_cycle && !msp_done && (curr.host != HOST_NONE)) && (lora_mode > LORA_SYNC)) { + +/* + if ((millis() > msp_next_cycle && msp_step < 3 && (curr.host != HOST_NONE)) && (lora_mode > LORA_SYNC)) { stats.timer_begin = millis(); - msp_send_peers(); - msp_set_state(); - msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); -// if (display_enabled == 1) { -// msp.request(MSP_ANALOG, &curr.vbat, sizeof(curr.vbat)); -// } + switch (msp_step) { + + case 0: + msp_send_peers(); + break; + + case 1: + msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); + break; + + case 2: + msp_set_state(); +// if (display_enabled == 1) { +// msp.request(MSP_ANALOG, &curr.vbat, sizeof(curr.vbat)); +// } + break; + + } stats.timer_end = millis(); - stats.last_msp_duration = stats.timer_end - stats.timer_begin; - msp_done = 1; + stats.last_msp_duration += stats.timer_end - stats.timer_begin; + + msp_next_cycle += MSP_CYCLE_DELAY; + msp_step++; } +*/ + // ---------------------- STATISTICS & IO @@ -603,9 +668,9 @@ void loop() { sys_pps = sys_ppsc; sys_ppsc = 0; now = millis(); - + // Pruning the timed-out peers - + for (int i = 0; i < LORA_MAXPEERS; i++) { if (peers[i].id > 0 && ((now - peers[i].updated) > LORA_PEER_TIMEOUT)) { peers[i].state = 0; @@ -618,9 +683,9 @@ void loop() { stats.packets_total += sys_num_peers * CYCLE_STATS / LORA_CYCLE; stats.packets_received += sys_pps; stats.percent_received = (stats.packets_received > 0) ? constrain(100 * stats.packets_received / stats.packets_total, 0 ,100) : 0; - + // Screen management - + if ((curr.state == 0) && (display_enabled == 0)) { // Aircraft is disarmed = Turning on the OLED display_enable(); } diff --git a/src/main.h b/src/main.h index 49cac6e..7dad0b6 100644 --- a/src/main.h +++ b/src/main.h @@ -15,7 +15,9 @@ #define LORA_CYCLE_TIMING_DELAY -16 // ms #define LORA_CYCLE_ANTIDRIFT_THRESHOLD 5 // ms #define LORA_CYCLE_ANTIDRIFT_CORRECTION 5 // ms -#define MSP_CYCLE_DELAY 50 // ms +#define MSP_CYCLE_DELAY 100 // ms + +#define LORA_NAME_LENGTH 5 #define SERIAL_PIN_TX 23 #define SERIAL_PIN_RX 17 @@ -45,34 +47,34 @@ char host_name[3][5]={"NoFC", "iNav", "Beta"}; struct peer_t { uint8_t id; uint8_t state; - char name[6]; + char name[LORA_NAME_LENGTH]; uint8_t host; uint8_t tick; uint32_t updated; int rssi; msp_raw_gps_t gps; -}; + }; struct curr_t { uint8_t id; uint8_t state; + uint8_t host; char name[16]; uint8_t tick; msp_raw_gps_t gps; msp_analog_t vbat; - uint8_t host; }; struct peer_air_t { uint8_t id; - char name[3]; uint8_t host; uint8_t tick; - int32_t lat; - int32_t lon; - int16_t alt; - int16_t speed; - int16_t heading; + uint8_t lat; + int32_t lon; + int16_t alt; + int16_t speed; + int16_t heading; + char name[LORA_NAME_LENGTH]; }; struct stats_t { From c96126ad5eec3b86050276a35aba53163dea0682 Mon Sep 17 00:00:00 2001 From: Olivier C Date: Sun, 21 Apr 2019 00:04:37 +0200 Subject: [PATCH 09/17] Packet length fixed --- src/main.cpp | 78 +++++++++++++++++++++++++++------------------------- src/main.h | 8 +++--- 2 files changed, 44 insertions(+), 42 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 2daaa65..3d30ca9 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -59,7 +59,7 @@ uint32_t now = 0; void lora_send(peer_air_t *outgoing) { while (!LoRa.beginPacket()) { } - LoRa.write((uint8_t*)outgoing, sizeof(outgoing)); + LoRa.write((uint8_t*)outgoing, sizeof(peerout)); LoRa.endPacket(false); } @@ -72,6 +72,7 @@ void onReceive(int packetSize) { sys_rssi = LoRa.packetRssi(); sys_ppsc++; + uint8_t id = incoming.id - 1; // if (String(incoming.name) != String(peers[id].name)) { // Something wrong, id slot / name mismatch @@ -94,7 +95,7 @@ void onReceive(int packetSize) { peers[id].tick = incoming.tick; strncpy(peers[id].name, incoming.name, LORA_NAME_LENGTH); - // peers[id].name[LORA_NAME_LENGTH - 1] = 0; + peers[id].name[LORA_NAME_LENGTH] = 0; stats.timer_end = millis(); stats.last_rx_duration = stats.timer_end - lora_last_rx; @@ -238,7 +239,7 @@ void display_draw() { display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString (0, line, String(peers[i].id) + ":"); display.drawString (16, line, String(peers[i].name)); - display.drawString (48, line, String(host_name[peers[i].host])); + display.drawString (56, line, String(host_name[peers[i].host])); display.setTextAlignment (TEXT_ALIGN_RIGHT); if (lora_last_tx > peers[i].updated) { @@ -295,33 +296,34 @@ void display_draw() { display.drawString (96, 0, String(host_name[curr.host])); display.setTextAlignment(TEXT_ALIGN_RIGHT); - display.drawString (120, 10, String((float)curr.gps.lat/10000000,6)); - display.drawString (120, 20, String((float)curr.gps.lon/10000000,6)); + display.drawString (120, 10, String((float)curr.gps.lat/10000000, 6)); + display.drawString (120, 20, String((float)curr.gps.lon/10000000, 6)); display.drawString (120, 30, String((float)curr.gps.alt)); display.drawString (120, 40, String(peerout.tick)); - } + } else if (sys_display_page == 4) { display.setFont (ArialMT_Plain_10); display.setTextAlignment (TEXT_ALIGN_LEFT); - int j = 0; - for (int i = 0; i < LORA_MAXPEERS ; i++) { - if (peers[i].id > 0 && j < 3) { + int j = 0; + for (int i = 0; i < LORA_MAXPEERS ; i++) { + if (peers[i].id > 0 && j < 3) { line = j * 20; display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString (0, line, String(peers[i].id) + ":"); display.drawString (13, line, String(peers[i].name)); - display.drawString (13, line + 10, "LQ" + String(peers[i].rssi)); - display.drawString (61, line + 10, String(peers[i].tick)); + display.drawString (13, line + 10, "LQ " + String(peers[i].rssi)); + display.drawString (58, line + 10, String(peers[i].tick)); display.setTextAlignment (TEXT_ALIGN_RIGHT); - display.drawString (128, line, String((float)peers[i].gps.lat/10000000,6)); - display.drawString (128, line + 10, String((float)peers[i].gps.lon/10000000,6)); + display.drawString (128, line, String((float)peers[i].gps.lat/10000000,5)); + display.drawString (128, line + 10, String((float)peers[i].gps.lon/10000000,5)); + j++; } - } + } } - last_received_id = 0; + last_received_id = 0; sys_message[0] = 0; display.display(); } @@ -340,22 +342,21 @@ void msp_set_name() { // } else { - for (int i = 0; i < LORA_NAME_LENGTH - 1; i++) { + for (int i = 0; i < LORA_NAME_LENGTH; i++) { curr.name[i] = (char) random(65, 90); } - curr.name[LORA_NAME_LENGTH - 1] = 0; + curr.name[LORA_NAME_LENGTH] = 0; } - } void msp_set_fc() { char j[5]; msp.request(MSP_FC_VARIANT, &j, sizeof(j)); - if (strcmp(j, "INAV") == 0) { + if (strncmp(j, "INAV", 4) == 0) { curr.host = HOST_INAV; } - else if (strcmp(j, "BTFL") == 0) { + else if (strncmp(j, "BTFL", 4) == 0) { curr.host = HOST_BTFL; } else { @@ -463,7 +464,11 @@ void setup() { curr.id = 0; strncpy(peerout.name, curr.name, LORA_NAME_LENGTH); peerout.host = curr.host; - + curr.gps.fixType = 0; + curr.gps.lat = 0; + curr.gps.lon = 0; + curr.gps.alt = 0; + LoRa.sleep(); LoRa.receive(); @@ -559,24 +564,21 @@ void loop() { if (lora_mode == LORA_TX) { - if (curr.host != HOST_NONE) { - - if (curr.gps.fixType > 0) { - peerout.lat = curr.gps.lat; - peerout.lon = curr.gps.lon; - peerout.alt = curr.gps.alt; - peerout.speed = curr.gps.groundSpeed; - peerout.heading = curr.gps.groundCourse; - } - else { - peerout.lat = 128; - peerout.lon = 222222222; - peerout.alt = 333; - peerout.speed = 0; - peerout.heading = 0; - } + if ((curr.host != HOST_NONE) && (curr.gps.fixType > 0)) { + peerout.lat = curr.gps.lat; + peerout.lon = curr.gps.lon; + peerout.alt = curr.gps.alt; + peerout.speed = curr.gps.groundSpeed; + peerout.heading = curr.gps.groundCourse; } - + else { + peerout.lat = 0; + peerout.lon = 0; + peerout.alt = 0; + peerout.speed = 0; + peerout.heading = 0; + } + peerout.tick++; stats.last_tx_begin = millis(); diff --git a/src/main.h b/src/main.h index 7dad0b6..f772531 100644 --- a/src/main.h +++ b/src/main.h @@ -17,7 +17,7 @@ #define LORA_CYCLE_ANTIDRIFT_CORRECTION 5 // ms #define MSP_CYCLE_DELAY 100 // ms -#define LORA_NAME_LENGTH 5 +#define LORA_NAME_LENGTH 4 #define SERIAL_PIN_TX 23 #define SERIAL_PIN_RX 17 @@ -28,7 +28,7 @@ #define LORA_MAXPEERS 5 -#define LORA_PEER_TIMEOUT 3000 // ms +#define LORA_PEER_TIMEOUT 4000 // ms #define SERIAL_FC_TIMEOUT 4000 // ms #define SCK 5 // GPIO5 - SX1278's SCK @@ -47,7 +47,7 @@ char host_name[3][5]={"NoFC", "iNav", "Beta"}; struct peer_t { uint8_t id; uint8_t state; - char name[LORA_NAME_LENGTH]; + char name[LORA_NAME_LENGTH + 1]; uint8_t host; uint8_t tick; uint32_t updated; @@ -69,7 +69,7 @@ struct peer_air_t { uint8_t id; uint8_t host; uint8_t tick; - uint8_t lat; + int32_t lat; int32_t lon; int16_t alt; int16_t speed; From 298b4769ab1b5abd4e225bdb9e3342f27e3e5a1b Mon Sep 17 00:00:00 2001 From: Olivier C Date: Sun, 21 Apr 2019 10:21:00 +0200 Subject: [PATCH 10/17] Few fixes, v0.9 --- src/main.cpp | 58 ++++++++++++++++++++++++++++++---------------------- src/main.h | 4 ++-- 2 files changed, 35 insertions(+), 27 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 3d30ca9..b65be93 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -52,6 +52,7 @@ uint32_t msp_next_cycle = 0; int msp_step = 0; uint32_t display_updated = 0; uint32_t now = 0; +int drift_correction = 0; // -------- LoRa @@ -168,6 +169,7 @@ void display_draw() { display.drawString (80, 35, String(peerout.tick)); display.setTextAlignment (TEXT_ALIGN_LEFT); + display.drawString (60, 00, String(sys_message)); display.drawString (60, 21, String(curr.name)); display.drawString (27, 12, "SAT"); display.drawString (108, 45, "%E"); @@ -260,24 +262,24 @@ void display_draw() { display.setFont (ArialMT_Plain_10); display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString(0, 0, "TX TIME"); - display.drawString(0, 10, "RX TIME"); - display.drawString(0, 20, "MSP TIME"); - display.drawString(0, 30, "OLED TIME"); - display.drawString(0, 40, "LORA CYCLE"); +// display.drawString(0, 10, "RX TIME"); + display.drawString(0, 10, "MSP TIME"); + display.drawString(0, 20, "OLED TIME"); + display.drawString(0, 30, "LORA CYCLE"); display.drawString(112, 0, "ms"); display.drawString(112, 10, "ms"); display.drawString(112, 20, "ms"); display.drawString(112, 30, "ms"); - display.drawString(112, 40, "ms"); +// display.drawString(112, 40, "ms"); display.setTextAlignment(TEXT_ALIGN_RIGHT); display.drawString (111, 0, String(stats.last_tx_duration)); // display.drawString (111, 10, String(stats.last_rx_duration)); - display.drawString (111, 10, "--"); - display.drawString (111, 20, String(stats.last_msp_duration)); - display.drawString (111, 30, String(stats.last_oled_duration)); - display.drawString (111, 40, String(LORA_CYCLE)); +// display.drawString (111, 10, "--"); + display.drawString (111, 10, String(stats.last_msp_duration)); + display.drawString (111, 20, String(stats.last_oled_duration)); + display.drawString (111, 30, String(LORA_CYCLE)); } @@ -307,14 +309,17 @@ void display_draw() { int j = 0; for (int i = 0; i < LORA_MAXPEERS ; i++) { - if (peers[i].id > 0 && j < 3) { - line = j * 20; + if (peers[i].id > 0 && j < 2) { + line = j * 30; display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString (0, line, String(peers[i].id) + ":"); display.drawString (13, line, String(peers[i].name)); + display.drawString (50, line, String(host_name[peers[i].host])); display.drawString (13, line + 10, "LQ " + String(peers[i].rssi)); - display.drawString (58, line + 10, String(peers[i].tick)); - + display.drawString (50, line + 10, String(peers[i].tick)); + display.drawString (13, line + 20, "S:" + String(peers[i].gps.groundSpeed)); + display.drawString (80, line + 20, "H:" + String(peers[i].gps.groundCourse / 10)); + display.setTextAlignment (TEXT_ALIGN_RIGHT); display.drawString (128, line, String((float)peers[i].gps.lat/10000000,5)); display.drawString (128, line + 10, String((float)peers[i].gps.lon/10000000,5)); @@ -344,8 +349,9 @@ void msp_set_name() { else { for (int i = 0; i < LORA_NAME_LENGTH; i++) { curr.name[i] = (char) random(65, 90); + curr.name[LORA_NAME_LENGTH] = 0; } - curr.name[LORA_NAME_LENGTH] = 0; + } } @@ -569,7 +575,7 @@ void loop() { peerout.lon = curr.gps.lon; peerout.alt = curr.gps.alt; peerout.speed = curr.gps.groundSpeed; - peerout.heading = curr.gps.groundCourse; + peerout.heading = curr.gps.groundCourse / 10; } else { peerout.lat = 0; @@ -586,21 +592,23 @@ void loop() { lora_last_tx = millis(); stats.last_tx_duration = lora_last_tx - stats.last_tx_begin; -// msp_step = 0; -// stats.last_msp_duration = 0; - //msp_next_cycle = lora_last_tx + LORA_CYCLE_SLOTSPACING / 2; // Begin sending MSP half a slot later + msp_step = 0; + stats.last_msp_duration = 0; + msp_next_cycle = stats.last_tx_begin + LORA_CYCLE_SLOTSPACING / 2; // Begin sending MSP half a slot later // Drift correction - if (peers[curr.id].id > 1 && peers[curr.id - 1].id > 0) { + if ((curr.id > 1) && (peers[curr.id - 1].id > 0)) { lora_drift = lora_last_tx - peers[curr.id - 1].updated - LORA_CYCLE_SLOTSPACING; - if ((abs(lora_drift) > LORA_CYCLE_ANTIDRIFT_THRESHOLD) && (abs(lora_drift) < LORA_CYCLE_SLOTSPACING * 0.5)) { - lora_next_tx += constrain(lora_drift, -LORA_CYCLE_ANTIDRIFT_CORRECTION, LORA_CYCLE_ANTIDRIFT_CORRECTION); + if ((abs(lora_drift) > LORA_CYCLE_ANTIDRIFT_THRESHOLD) && (abs(lora_drift) < (LORA_CYCLE_SLOTSPACING * 0.5))) { + drift_correction = constrain(lora_drift, -LORA_CYCLE_ANTIDRIFT_CORRECTION, LORA_CYCLE_ANTIDRIFT_CORRECTION); + lora_next_tx += drift_correction; + sprintf(sys_message, "%s %3d", "DRIFT:", drift_correction); } } - +/* if (curr.host != HOST_NONE) { stats.timer_begin = millis(); msp_send_peers(); @@ -609,6 +617,7 @@ void loop() { stats.timer_end = millis(); stats.last_msp_duration = stats.timer_end - stats.timer_begin; } +*/ // Back to RX @@ -631,8 +640,8 @@ void loop() { // ---------------------- SERIAL / MSP -/* - if ((millis() > msp_next_cycle && msp_step < 3 && (curr.host != HOST_NONE)) && (lora_mode > LORA_SYNC)) { + + if (((millis() > msp_next_cycle) && (msp_step < 3) && (curr.host != HOST_NONE)) && (lora_mode > LORA_SYNC)) { stats.timer_begin = millis(); switch (msp_step) { @@ -660,7 +669,6 @@ void loop() { msp_next_cycle += MSP_CYCLE_DELAY; msp_step++; } -*/ // ---------------------- STATISTICS & IO diff --git a/src/main.h b/src/main.h index f772531..5486d9b 100644 --- a/src/main.h +++ b/src/main.h @@ -1,4 +1,4 @@ -#define VERSION "O.0.9" +#define VERSION "0.9" #define LORA_INIT 0 #define LORA_SYNC 1 @@ -28,7 +28,7 @@ #define LORA_MAXPEERS 5 -#define LORA_PEER_TIMEOUT 4000 // ms +#define LORA_PEER_TIMEOUT 5000 // ms #define SERIAL_FC_TIMEOUT 4000 // ms #define SCK 5 // GPIO5 - SX1278's SCK From 10846e4f0b3ecfb74da2d7b8ef6ce27b49981e5e Mon Sep 17 00:00:00 2001 From: Olivier C Date: Tue, 23 Apr 2019 11:52:41 +0200 Subject: [PATCH 11/17] Anti-slot-collision + performance tweaks --- src/main.cpp | 442 +++++++++++++++++++++++++++++---------------------- src/main.h | 68 ++++---- 2 files changed, 283 insertions(+), 227 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index b65be93..cbbac4f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -11,15 +11,17 @@ SSD1306 display(0x3c, 4, 15); +config_t cfg; +MSP msp; + curr_t curr; // Our peer peer_t peers[LORA_MAXPEERS]; // Other peers peer_air_t incoming; // Received peer peer_air_t peerout; // Sent peer -MSP msp; stats_t stats; int sys_rssi; -uint8_t lora_mode; +uint8_t main_mode; uint8_t last_received_id = 0; char sys_message[20]; @@ -42,7 +44,6 @@ bool display_enabled = 1; uint32_t cycle_scan_begin; uint32_t last_tx_begin; uint32_t last_tx_end; -uint32_t lora_origin = 0; uint32_t lora_last_tx = 0; uint32_t lora_next_tx = 0; uint32_t lora_last_rx = 0; @@ -54,7 +55,67 @@ uint32_t display_updated = 0; uint32_t now = 0; int drift_correction = 0; +// -------- SYSTEM + +void set_mode(uint8_t mode) { + + switch (mode) { + + case 0 : + cfg.lora_frequency = 433E6; + cfg.lora_bandwidth = 250000; + cfg.lora_coding_rate = 5; + cfg.lora_spreading_factor = 8; + cfg.lora_power = 20; + cfg.lora_cycle = 500; + cfg.lora_slot_spacing = 100; + cfg.lora_timing_delay = -60; + cfg.lora_antidrift_threshold = 5; + cfg.lora_antidrift_correction = 5; + cfg.lora_peer_timeout = 5000; + + cfg.msp_cycle_delay = 100; + cfg.msp_fc_timeout = 7000; + + cfg.cycle_scan = 4000; + cfg.cycle_display = 125; + cfg.cycle_stats = 1000; + + break; + } +} + +int count_peers() { + int j = 0; + for (int i = 0; i < LORA_MAXPEERS; i++) { + if (peers[i].id > 0) { + j++; + } + } + return j; +} + +void pick_id() { + curr.id = 0; + for (int i = 0; i < LORA_MAXPEERS; i++) { + if ((peers[i].id == 0) && (curr.id == 0)) { + curr.id = i + 1; + peerout.id = curr.id; + } + } +} +void resync_tx_slot(int16_t delay) { + bool startnow = 0; + for (int i = 0; (i < LORA_MAXPEERS) && (startnow == 0); i++) { // Resync + if (peers[i].id > 0) { + lora_next_tx = peers[i].updated + (curr.id - peers[i].id) * cfg.lora_slot_spacing + cfg.lora_cycle + delay; + startnow = 1; + } + } +} + + // -------- LoRa void lora_send(peer_air_t *outgoing) { @@ -65,7 +126,7 @@ void lora_send(peer_air_t *outgoing) { } -void onReceive(int packetSize) { +void lora_receive(int packetSize) { if (packetSize == 0) return; lora_last_rx = millis(); @@ -76,9 +137,6 @@ void onReceive(int packetSize) { uint8_t id = incoming.id - 1; -// if (String(incoming.name) != String(peers[id].name)) { // Something wrong, id slot / name mismatch -// peers[incoming.id].state = 3; -// } peers[id].id = incoming.id; last_received_id = incoming.id; @@ -100,6 +158,16 @@ void onReceive(int packetSize) { stats.timer_end = millis(); stats.last_rx_duration = stats.timer_end - lora_last_rx; + + if (incoming.id == curr.id) { // Same slot, conflict + uint32_t cs1 = incoming.name[0] + incoming.name[1] * 26 + incoming.name[2] * 26 * 26 ; + uint32_t cs2 = curr.name[0] + curr.name[1] * 26 + curr.name[2] * 26 * 26; + if (cs1 < cs2) { // Pick another slot + sprintf(sys_message, "%s", "ID CONFLICT"); + pick_id(); + resync_tx_slot(cfg.lora_timing_delay); + } + } } void lora_init() { @@ -107,19 +175,19 @@ void lora_init() { SPI.begin(5, 19, 27, 18); LoRa.setPins(SS, RST, DI0); - if (!LoRa.begin(LORA_FREQUENCY)) { + if (!LoRa.begin(cfg.lora_frequency)) { display.drawString (94, 9, "FAIL"); while (1); } LoRa.sleep(); - LoRa.setSignalBandwidth(LORA_BANDWIDTH); - LoRa.setCodingRate4(LORA_CODING_RATE); - LoRa.setSpreadingFactor(LORA_SPREADING_FACTOR); - LoRa.setTxPower(LORA_POWER, 1); + LoRa.setSignalBandwidth(cfg.lora_bandwidth); + LoRa.setCodingRate4(cfg.lora_coding_rate); + LoRa.setSpreadingFactor(cfg.lora_spreading_factor); + LoRa.setTxPower(cfg.lora_power, 1); LoRa.setOCP(250); LoRa.idle(); - LoRa.onReceive(onReceive); + LoRa.onReceive(lora_receive); LoRa.enableCrc(); } @@ -200,14 +268,64 @@ void display_draw() { else if (sys_display_page == 1) { + display.setFont (ArialMT_Plain_10); + display.setTextAlignment (TEXT_ALIGN_LEFT); + + display.drawString(0, 0, "HOST:"); + display.drawString(0, 10, "GPS LAT"); + display.drawString(0, 20, "GPS LON"); + display.drawString(0, 30, "GPS ALT"); + display.drawString(0, 40, "TICK"); + display.drawString (32, 0, String(curr.name)); + display.drawString (96, 0, String(host_name[curr.host])); + + display.setTextAlignment(TEXT_ALIGN_RIGHT); + display.drawString (120, 10, String((float)curr.gps.lat/10000000, 6)); + display.drawString (120, 20, String((float)curr.gps.lon/10000000, 6)); + display.drawString (120, 30, String((float)curr.gps.alt)); + display.drawString (120, 40, String(peerout.tick)); + + } + + else if (sys_display_page == 2) { + + display.setFont (ArialMT_Plain_10); + display.setTextAlignment (TEXT_ALIGN_LEFT); + + int j = 0; + for (int i = 0; i < LORA_MAXPEERS ; i++) { + if (peers[i].id > 0 && j < 3) { + line = j * 20; + display.setTextAlignment (TEXT_ALIGN_LEFT); + display.drawString (0, line, String(peers[i].id) + ":"); + display.drawString (13, line, String(peers[i].name)); + display.drawString (50, line, String(host_name[peers[i].host])); + display.drawString (13, line + 10, "LQ " + String(peers[i].rssi)); + display.drawString (50, line + 10, String(peers[i].tick)); +// display.drawString (13, line + 20, "S:" + String(peers[i].gps.groundSpeed)); +// display.drawString (80, line + 20, "H:" + String(peers[i].gps.groundCourse / 10)); + + display.setTextAlignment (TEXT_ALIGN_RIGHT); + display.drawString (128, line, String((float)peers[i].gps.lat/10000000,5)); + display.drawString (128, line + 10, String((float)peers[i].gps.lon/10000000,5)); + + j++; + } + } + if (j == 0) { + display.drawString (0, 0, "NO PEER DETECTED"); + } + } + else if (sys_display_page == 3) { + display.setFont (ArialMT_Plain_10); display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString (0, 1 , "_____________________"); display.drawString (3, 4 , "."); display.drawString (60, 4 , "."); display.drawString (122, 4 , "."); - display.drawString (0, 14 , String(LORA_CYCLE)); - display.drawString (52, 14 , String(LORA_CYCLE / 2)); + display.drawString (0, 14 , String(cfg.lora_cycle)); + display.drawString (52, 14 , String(cfg.lora_cycle / 2)); display.drawString (120, 14 , "0"); long pos[LORA_MAXPEERS]; @@ -217,8 +335,8 @@ void display_draw() { for (int i = 0; i < LORA_MAXPEERS ; i++) { if (peers[i].id != 0) { diff = now - peers[i].updated; - if ( diff < LORA_CYCLE) { - pos[i] = 120 - 120 * diff / LORA_CYCLE; + if ( diff < cfg.lora_cycle) { + pos[i] = 120 - 120 * diff / cfg.lora_cycle; } } else { @@ -227,8 +345,8 @@ void display_draw() { } diff = now - lora_last_tx; - if ( diff < LORA_CYCLE) { - display.drawString (120 - 120 * diff / LORA_CYCLE, 0, String(curr.id)); + if ( diff < cfg.lora_cycle) { + display.drawString (120 - 120 * diff / cfg.lora_cycle, 0, String(curr.id)); } for (int i = 0; i < LORA_MAXPEERS ; i++) { @@ -255,78 +373,34 @@ void display_draw() { j++; } } - } - - else if (sys_display_page == 2) { + } + else if (sys_display_page == 4) { + stats.last_rx_duration = cfg.lora_cycle - stats.last_tx_duration - stats.last_msp_tx_duration - stats.last_msp_rx_duration - stats.last_oled_duration * cfg.lora_cycle / cfg.cycle_display; + display.setFont (ArialMT_Plain_10); display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString(0, 0, "TX TIME"); -// display.drawString(0, 10, "RX TIME"); - display.drawString(0, 10, "MSP TIME"); - display.drawString(0, 20, "OLED TIME"); - display.drawString(0, 30, "LORA CYCLE"); - + display.drawString(0, 10, "MSP TX TIME"); + display.drawString(0, 20, "MSP RX TIME"); + display.drawString(0, 30, "OLED TIME"); + display.drawString(0, 40, "RX+IDLE TIME"); + display.drawString(0, 50, "LORA CYCLE"); + display.drawString(112, 0, "ms"); display.drawString(112, 10, "ms"); display.drawString(112, 20, "ms"); display.drawString(112, 30, "ms"); -// display.drawString(112, 40, "ms"); - + display.drawString(112, 40, "ms"); + display.drawString(112, 50, "ms"); + display.setTextAlignment(TEXT_ALIGN_RIGHT); display.drawString (111, 0, String(stats.last_tx_duration)); - // display.drawString (111, 10, String(stats.last_rx_duration)); -// display.drawString (111, 10, "--"); - display.drawString (111, 10, String(stats.last_msp_duration)); - display.drawString (111, 20, String(stats.last_oled_duration)); - display.drawString (111, 30, String(LORA_CYCLE)); - - } - - else if (sys_display_page == 3) { - display.setFont (ArialMT_Plain_10); - display.setTextAlignment (TEXT_ALIGN_LEFT); - - - display.drawString(0, 0, "HOST:"); - display.drawString(0, 10, "GPS LAT"); - display.drawString(0, 20, "GPS LON"); - display.drawString(0, 30, "GPS ALT"); - display.drawString(0, 40, "TICK"); - - display.drawString (32, 0, String(curr.name)); - display.drawString (96, 0, String(host_name[curr.host])); - - display.setTextAlignment(TEXT_ALIGN_RIGHT); - display.drawString (120, 10, String((float)curr.gps.lat/10000000, 6)); - display.drawString (120, 20, String((float)curr.gps.lon/10000000, 6)); - display.drawString (120, 30, String((float)curr.gps.alt)); - display.drawString (120, 40, String(peerout.tick)); - } - else if (sys_display_page == 4) { - display.setFont (ArialMT_Plain_10); - display.setTextAlignment (TEXT_ALIGN_LEFT); - - int j = 0; - for (int i = 0; i < LORA_MAXPEERS ; i++) { - if (peers[i].id > 0 && j < 2) { - line = j * 30; - display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (0, line, String(peers[i].id) + ":"); - display.drawString (13, line, String(peers[i].name)); - display.drawString (50, line, String(host_name[peers[i].host])); - display.drawString (13, line + 10, "LQ " + String(peers[i].rssi)); - display.drawString (50, line + 10, String(peers[i].tick)); - display.drawString (13, line + 20, "S:" + String(peers[i].gps.groundSpeed)); - display.drawString (80, line + 20, "H:" + String(peers[i].gps.groundCourse / 10)); - - display.setTextAlignment (TEXT_ALIGN_RIGHT); - display.drawString (128, line, String((float)peers[i].gps.lat/10000000,5)); - display.drawString (128, line + 10, String((float)peers[i].gps.lon/10000000,5)); - - j++; - } - } + display.drawString (111, 10, String(stats.last_msp_tx_duration)); + display.drawString (111, 20, String(stats.last_msp_rx_duration)); + display.drawString (111, 30, String(stats.last_oled_duration)); + display.drawString (111, 40, String(stats.last_rx_duration)); + display.drawString (111, 50, String(cfg.lora_cycle)); } last_received_id = 0; sys_message[0] = 0; @@ -343,16 +417,7 @@ void msp_set_state() { } void msp_set_name() { - if (msp.request(MSP_NAME, &curr.name, sizeof(curr.name))) { - // - } - else { - for (int i = 0; i < LORA_NAME_LENGTH; i++) { - curr.name[i] = (char) random(65, 90); - curr.name[LORA_NAME_LENGTH] = 0; - } - - } + msp.request(MSP_NAME, &curr.name, sizeof(curr.name)); } void msp_set_fc() { @@ -413,22 +478,13 @@ void IRAM_ATTR handleInterrupt() { portEXIT_CRITICAL_ISR(&mux); } -// -------- SYSTEM - -int count_peers() { - int j = 0; - for (int i = 0; i < LORA_MAXPEERS; i++) { - if (peers[i].id > 0) { - j++; - } - } - return j; -} // ----------------------------- void setup() { + set_mode(0); + display_init(); display.drawString(0, 0, "RADAR VERSION"); @@ -448,103 +504,116 @@ void setup() { display.drawString (90, 18, "OK"); display.display(); - display.drawString (0, 27, "HOST"); - display.display(); - delay(SERIAL_FC_TIMEOUT); - msp_set_name(); - msp_set_fc(); - display.drawString (90, 27, String(host_name[curr.host])); - display.display(); + for (int i = 0; i < LORA_MAXPEERS; i++) { + peers[i].id = 0; + } pinMode(interruptPin, INPUT); io_button_pressed = 0; attachInterrupt(digitalPinToInterrupt(interruptPin), handleInterrupt, RISING); - display.drawString (0, 38, "SCAN"); + display.drawString (0, 27, "HOST"); display.display(); - for (int i = 0; i < LORA_MAXPEERS; i++) { - peers[i].id = 0; - } - - curr.id = 0; - strncpy(peerout.name, curr.name, LORA_NAME_LENGTH); - peerout.host = curr.host; - curr.gps.fixType = 0; - curr.gps.lat = 0; - curr.gps.lon = 0; - curr.gps.alt = 0; - - LoRa.sleep(); - LoRa.receive(); - display_updated = 0; cycle_scan_begin = millis(); - lora_mode = LORA_INIT; + curr.host = HOST_NONE; + + main_mode = MODE_HOST_SCAN; } // ----------------------------------------------------------------------------- MAIN LOOP void loop() { - // ---------------------- IO BUTTON if ((millis() > io_button_released + 150) && (io_button_pressed == 1)) { io_button_pressed = 0; } -// ---------------------- LORA INIT +// ---------------------- HOST SCAN - if (lora_mode == LORA_INIT) { - if (millis() > (cycle_scan_begin + CYCLE_SCAN)) { // End of the scan, set the ID then sync - for (int i = 0; i < LORA_MAXPEERS; i++) { - if ((peers[i].id == 0)) { - if (curr.id == 0) { - curr.id = i + 1; - peerout.id = curr.id; - } + if (main_mode == MODE_HOST_SCAN) { + if ((millis() > (cycle_scan_begin + cfg.msp_fc_timeout)) || (curr.host != HOST_NONE)) { // End of the host scan + + if (curr.host != HOST_NONE) { + msp_set_name(); + } + else { + for (int i = 0; i < LORA_NAME_LENGTH; i++) { + curr.name[i] = (char) random(65, 90); + curr.name[LORA_NAME_LENGTH] = 0; } } - lora_mode = LORA_SYNC; + + strncpy(peerout.name, curr.name, LORA_NAME_LENGTH); + peerout.host = curr.host; + curr.gps.fixType = 0; + curr.gps.lat = 0; + curr.gps.lon = 0; + curr.gps.alt = 0; + curr.id = 0; + + display.drawString (90, 27, String(host_name[curr.host])); + display.drawString (0, 38, "SCAN"); + display.display(); + + LoRa.sleep(); + LoRa.receive(); + + cycle_scan_begin = millis(); + main_mode = MODE_LORA_INIT; + } else { // Still scanning - if ((millis() > display_updated + CYCLE_DISPLAY) && display_enabled) { + if ((millis() > display_updated + cfg.cycle_display / 2) && display_enabled) { + + delay(cfg.cycle_display / 2); + msp_set_fc(); + + display.drawProgressBar(35, 30, 48, 6, 100 * (millis() - cycle_scan_begin) / cfg.msp_fc_timeout); + display.display(); + display_updated = millis(); + } + } + } + +// ---------------------- LORA INIT + + if (main_mode == MODE_LORA_INIT) { + if (millis() > (cycle_scan_begin + cfg.cycle_scan)) { // End of the scan, set the ID then sync + pick_id(); + main_mode = MODE_LORA_SYNC; + } else { // Still scanning + if ((millis() > display_updated + cfg.cycle_display / 2) && display_enabled) { for (int i = 0; i < LORA_MAXPEERS; i++) { if (peers[i].id > 0) { display.drawString(40 + peers[i].id * 8, 38, String(peers[i].id)); } } - display.drawProgressBar(00, 53, 126, 7, 100 * (millis() - cycle_scan_begin) / CYCLE_SCAN); + display.drawProgressBar(0, 53, 126, 6, 100 * (millis() - cycle_scan_begin) / cfg.cycle_scan); display.display(); display_updated = millis(); } } } -// ---------------------- LORA START +// ---------------------- LORA SYNC - if (lora_mode == LORA_SYNC) { + if (main_mode == MODE_LORA_SYNC) { sys_num_peers = count_peers(); - lora_origin = millis(); if (sys_num_peers == 0) { // Alone, start at will - lora_next_tx = lora_origin + LORA_CYCLE; + lora_next_tx = millis() + cfg.lora_cycle; } else { // Not alone, sync by slot - - bool startnow = 0; - for (int i = 0; (i < LORA_MAXPEERS) && (startnow == 0); i++) { - if (peers[i].id > 0) { - lora_next_tx = peers[i].updated + (curr.id - peers[i].id) * LORA_CYCLE_SLOTSPACING + LORA_CYCLE + LORA_CYCLE_TIMING_DELAY; - startnow = 1; - } - } - + resync_tx_slot(cfg.lora_timing_delay); } display_updated = millis(); stats_updated = millis(); + sys_pps = 0; sys_ppsc = 0; sys_num_peers = 0; @@ -552,23 +621,24 @@ void loop() { stats.packets_received = 0; stats.percent_received = 0; peerout.tick = 0; - lora_mode = LORA_RX; + + main_mode = MODE_LORA_RX; } // ---------------------- LORA RX - if ((lora_mode == LORA_RX) && (millis() > lora_next_tx)) { + if ((main_mode == MODE_LORA_RX) && (millis() > lora_next_tx)) { now = millis(); while (now > lora_next_tx) { // In case we skipped some beats - lora_next_tx += LORA_CYCLE; + lora_next_tx += cfg.lora_cycle; } - lora_mode = LORA_TX; + main_mode = MODE_LORA_TX; } // ---------------------- LORA TX - if (lora_mode == LORA_TX) { + if (main_mode == MODE_LORA_TX) { if ((curr.host != HOST_NONE) && (curr.gps.fixType > 0)) { peerout.lat = curr.gps.lat; @@ -584,7 +654,7 @@ void loop() { peerout.speed = 0; peerout.heading = 0; } - + peerout.tick++; stats.last_tx_begin = millis(); @@ -592,43 +662,31 @@ void loop() { lora_last_tx = millis(); stats.last_tx_duration = lora_last_tx - stats.last_tx_begin; - msp_step = 0; - stats.last_msp_duration = 0; - msp_next_cycle = stats.last_tx_begin + LORA_CYCLE_SLOTSPACING / 2; // Begin sending MSP half a slot later - // Drift correction - if ((curr.id > 1) && (peers[curr.id - 1].id > 0)) { - lora_drift = lora_last_tx - peers[curr.id - 1].updated - LORA_CYCLE_SLOTSPACING; + if ((curr.id > 1) && (peers[curr.id - 1].id > 0)) { + lora_drift = lora_last_tx - peers[curr.id - 1].updated - cfg.lora_slot_spacing; - if ((abs(lora_drift) > LORA_CYCLE_ANTIDRIFT_THRESHOLD) && (abs(lora_drift) < (LORA_CYCLE_SLOTSPACING * 0.5))) { - drift_correction = constrain(lora_drift, -LORA_CYCLE_ANTIDRIFT_CORRECTION, LORA_CYCLE_ANTIDRIFT_CORRECTION); + if ((abs(lora_drift) > cfg.lora_antidrift_threshold) && (abs(lora_drift) < (cfg.lora_slot_spacing * 0.5))) { + drift_correction = constrain(lora_drift, -cfg.lora_antidrift_correction, cfg.lora_antidrift_correction); lora_next_tx += drift_correction; sprintf(sys_message, "%s %3d", "DRIFT:", drift_correction); } } -/* - if (curr.host != HOST_NONE) { - stats.timer_begin = millis(); - msp_send_peers(); - msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); - msp_set_state(); - stats.timer_end = millis(); - stats.last_msp_duration = stats.timer_end - stats.timer_begin; - } -*/ + msp_step = 0; + msp_next_cycle = stats.last_tx_begin + cfg.lora_slot_spacing * 2.5; // Back to RX LoRa.sleep(); LoRa.receive(); - lora_mode = LORA_RX; + main_mode = MODE_LORA_RX; } // ---------------------- DISPLAY - if ((millis() > display_updated + CYCLE_DISPLAY) && display_enabled && (lora_mode > LORA_SYNC)) { + if ((millis() > display_updated + cfg.cycle_display) && display_enabled && (main_mode > MODE_LORA_SYNC)) { stats.timer_begin = millis(); display_draw(); @@ -640,40 +698,38 @@ void loop() { // ---------------------- SERIAL / MSP - - if (((millis() > msp_next_cycle) && (msp_step < 3) && (curr.host != HOST_NONE)) && (lora_mode > LORA_SYNC)) { - stats.timer_begin = millis(); + if (((millis() > msp_next_cycle) && (msp_step < 3) && (curr.host != HOST_NONE)) && (main_mode > MODE_LORA_SYNC)) { switch (msp_step) { case 0: - msp_send_peers(); + msp_set_state(); break; case 1: + stats.timer_begin = millis(); msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); + stats.timer_end = millis(); + stats.last_msp_rx_duration = stats.timer_end - stats.timer_begin; break; case 2: - msp_set_state(); -// if (display_enabled == 1) { -// msp.request(MSP_ANALOG, &curr.vbat, sizeof(curr.vbat)); -// } + stats.timer_begin = millis(); + msp_send_peers(); + stats.timer_end = millis(); + stats.last_msp_tx_duration = stats.timer_end - stats.timer_begin; break; } - stats.timer_end = millis(); - stats.last_msp_duration += stats.timer_end - stats.timer_begin; - - msp_next_cycle += MSP_CYCLE_DELAY; + msp_next_cycle += cfg.msp_cycle_delay; msp_step++; } // ---------------------- STATISTICS & IO - if ((millis() > (CYCLE_STATS + stats_updated)) && (lora_mode > LORA_SYNC)) { + if ((millis() > (cfg.cycle_stats + stats_updated)) && (main_mode > MODE_LORA_SYNC)) { sys_pps = sys_ppsc; sys_ppsc = 0; @@ -682,7 +738,7 @@ void loop() { // Pruning the timed-out peers for (int i = 0; i < LORA_MAXPEERS; i++) { - if (peers[i].id > 0 && ((now - peers[i].updated) > LORA_PEER_TIMEOUT)) { + if (peers[i].id > 0 && ((now - peers[i].updated) > cfg.lora_peer_timeout)) { peers[i].state = 0; peers[i].id = 0; sys_rssi = 0; @@ -690,7 +746,7 @@ void loop() { } sys_num_peers = count_peers(); - stats.packets_total += sys_num_peers * CYCLE_STATS / LORA_CYCLE; + stats.packets_total += sys_num_peers * cfg.cycle_stats / cfg.lora_cycle; stats.packets_received += sys_pps; stats.percent_received = (stats.packets_received > 0) ? constrain(100 * stats.packets_received / stats.packets_total, 0 ,100) : 0; diff --git a/src/main.h b/src/main.h index 5486d9b..5fadbec 100644 --- a/src/main.h +++ b/src/main.h @@ -1,36 +1,18 @@ -#define VERSION "0.9" +#define VERSION "0.91" -#define LORA_INIT 0 -#define LORA_SYNC 1 -#define LORA_RX 2 -#define LORA_TX 3 - -#define LORA_FREQUENCY 433E6 -#define LORA_BANDWIDTH 250000 -#define LORA_CODING_RATE 5 -#define LORA_SPREADING_FACTOR 7 -#define LORA_POWER 20 -#define LORA_CYCLE 500 // ms -#define LORA_CYCLE_SLOTSPACING 100 // ms -#define LORA_CYCLE_TIMING_DELAY -16 // ms -#define LORA_CYCLE_ANTIDRIFT_THRESHOLD 5 // ms -#define LORA_CYCLE_ANTIDRIFT_CORRECTION 5 // ms -#define MSP_CYCLE_DELAY 100 // ms +#define MODE_HOST_SCAN 0 +#define MODE_LORA_INIT 1 +#define MODE_LORA_SYNC 2 +#define MODE_LORA_RX 3 +#define MODE_LORA_TX 4 #define LORA_NAME_LENGTH 4 #define SERIAL_PIN_TX 23 #define SERIAL_PIN_RX 17 -#define CYCLE_SCAN 4000 // ms -#define CYCLE_DISPLAY 125 // ms -#define CYCLE_STATS 1000 // ms - #define LORA_MAXPEERS 5 -#define LORA_PEER_TIMEOUT 5000 // ms -#define SERIAL_FC_TIMEOUT 4000 // ms - #define SCK 5 // GPIO5 - SX1278's SCK #define MISO 19 // GPIO19 - SX1278's MISO #define MOSI 27 // GPIO27 - SX1278's MOSI @@ -48,7 +30,7 @@ struct peer_t { uint8_t id; uint8_t state; char name[LORA_NAME_LENGTH + 1]; - uint8_t host; + uint8_t host; uint8_t tick; uint32_t updated; int rssi; @@ -69,27 +51,45 @@ struct peer_air_t { uint8_t id; uint8_t host; uint8_t tick; - int32_t lat; + int32_t lat; int32_t lon; - int16_t alt; - int16_t speed; - int16_t heading; + int16_t alt; + int16_t speed; + int16_t heading; char name[LORA_NAME_LENGTH]; }; struct stats_t { uint32_t timer_begin; - uint32_t timer_end; + uint32_t timer_end; float packets_total; uint32_t packets_received; uint8_t percent_received; uint16_t last_tx_begin; uint16_t last_tx_duration; uint16_t last_rx_duration; - uint16_t last_msp_duration; + uint16_t last_msp_tx_duration; + uint16_t last_msp_rx_duration; uint16_t last_oled_duration; }; - - - +struct config_t { + uint32_t lora_frequency; + uint32_t lora_bandwidth; + uint8_t lora_coding_rate; + uint8_t lora_spreading_factor; + uint8_t lora_power; + uint16_t lora_cycle; + uint16_t lora_slot_spacing; + int16_t lora_timing_delay; + uint8_t lora_antidrift_threshold; + uint8_t lora_antidrift_correction; + uint16_t lora_peer_timeout; + + uint16_t msp_cycle_delay; + uint16_t msp_fc_timeout; + + uint16_t cycle_scan; + uint16_t cycle_display; + uint16_t cycle_stats; +}; From bc9474c98599ee483638cd6cbcaefbc5a7de2747 Mon Sep 17 00:00:00 2001 From: Olivier C Date: Wed, 24 Apr 2019 20:57:05 +0200 Subject: [PATCH 12/17] v0.91 --- src/lib/MSP.h | 4 +- src/main.cpp | 295 ++++++++++++++++++++++++++++++-------------------- src/main.h | 50 ++++++--- 3 files changed, 210 insertions(+), 139 deletions(-) diff --git a/src/lib/MSP.h b/src/lib/MSP.h index c9eaf50..37dd80f 100644 --- a/src/lib/MSP.h +++ b/src/lib/MSP.h @@ -650,11 +650,11 @@ struct msp_radar_pos_t { uint8_t id; uint8_t state; // disarmed(0) armed (1) int32_t lat; // decimal degrees latitude * 10000000 - int32_t lon; // decimal degrees latitude * 10000000 + int32_t lon; // decimal degrees longitude * 10000000 int32_t alt; // cm uint16_t heading; // deg uint16_t speed; // cm/s - uint8_t tick; // rssi + uint8_t lq; // lq } __attribute__((packed)); struct msp_radar_itd_t { diff --git a/src/main.cpp b/src/main.cpp index cbbac4f..624b0a9 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -15,9 +15,10 @@ config_t cfg; MSP msp; curr_t curr; // Our peer -peer_t peers[LORA_MAXPEERS]; // Other peers -peer_air_t incoming; // Received peer -peer_air_t peerout; // Sent peer +peer_t peers[LORA_NODES]; // Other peers +air_type0_t air_0; +air_type1_t air_1; +air_type1_t * air_r; stats_t stats; int sys_rssi; @@ -30,6 +31,7 @@ char sys_message[20]; uint8_t sys_pps = 0; uint8_t sys_ppsc = 0; int sys_num_peers = 0; +uint8_t sys_lora_sent; // --- Inputs outputs @@ -62,22 +64,22 @@ void set_mode(uint8_t mode) { switch (mode) { case 0 : - cfg.lora_frequency = 433E6; + cfg.lora_frequency = 433E6; // 433E6, 868E6, 915E6 cfg.lora_bandwidth = 250000; cfg.lora_coding_rate = 5; - cfg.lora_spreading_factor = 8; + cfg.lora_spreading_factor = 9; cfg.lora_power = 20; cfg.lora_cycle = 500; - cfg.lora_slot_spacing = 100; + cfg.lora_slot_spacing = 125; cfg.lora_timing_delay = -60; cfg.lora_antidrift_threshold = 5; cfg.lora_antidrift_correction = 5; cfg.lora_peer_timeout = 5000; - cfg.msp_cycle_delay = 100; cfg.msp_fc_timeout = 7000; - - cfg.cycle_scan = 4000; + cfg.msp_after_tx_delay = 85; + + cfg.cycle_scan = 3000; cfg.cycle_display = 125; cfg.cycle_stats = 1000; @@ -87,7 +89,7 @@ void set_mode(uint8_t mode) { int count_peers() { int j = 0; - for (int i = 0; i < LORA_MAXPEERS; i++) { + for (int i = 0; i < LORA_NODES; i++) { if (peers[i].id > 0) { j++; } @@ -95,19 +97,34 @@ int count_peers() { return j; } +void reset_peers() { + now = millis(); + for (int i = 0; i < LORA_NODES; i++) { + peers[i].id = 0; + peers[i].host = 0; + peers[i].state = 0; + peers[i].broadcast = 0; + peers[i].lq_updated = now; + peers[i].lq_tick = 0; + peers[i].lq = 0; + peers[i].updated = 0; + peers[i].rssi = 0; + strcpy(peers[i].name, "----"); + } +} + void pick_id() { curr.id = 0; - for (int i = 0; i < LORA_MAXPEERS; i++) { + for (int i = 0; i < LORA_NODES; i++) { if ((peers[i].id == 0) && (curr.id == 0)) { curr.id = i + 1; - peerout.id = curr.id; } } } -void resync_tx_slot(int16_t delay) { +void resync_tx_slot(int16_t delay) { bool startnow = 0; - for (int i = 0; (i < LORA_MAXPEERS) && (startnow == 0); i++) { // Resync + for (int i = 0; (i < LORA_NODES) && (startnow == 0); i++) { // Resync if (peers[i].id > 0) { lora_next_tx = peers[i].updated + (curr.id - peers[i].id) * cfg.lora_slot_spacing + cfg.lora_cycle + delay; startnow = 1; @@ -115,53 +132,85 @@ void resync_tx_slot(int16_t delay) { } } - // -------- LoRa -void lora_send(peer_air_t *outgoing) { +void lora_send() { + + sys_lora_sent++; - while (!LoRa.beginPacket()) { } - LoRa.write((uint8_t*)outgoing, sizeof(peerout)); - LoRa.endPacket(false); + if (sys_lora_sent % 10 == 0) { + air_1.id = curr.id; + air_1.type = 1; + air_1.host = curr.host; + air_1.state = 0; + air_1.broadcast = 0; + air_1.speed = curr.gps.groundSpeed / 100; + strncpy(air_1.name, curr.name, LORA_NAME_LENGTH); + while (!LoRa.beginPacket()) { } + LoRa.write((uint8_t*)&air_1, sizeof(air_1)); + LoRa.endPacket(false); + } + else { + air_0.id = curr.id; + air_0.type = 0; + air_0.lat = curr.gps.lat / 100; + air_0.lon = curr.gps.lon / 100; + air_0.alt = curr.gps.alt / 100; + air_0.heading = curr.gps.groundCourse / 10; + + while (!LoRa.beginPacket()) { } + LoRa.write((uint8_t*)&air_0, sizeof(air_0)); + LoRa.endPacket(false); + } } void lora_receive(int packetSize) { if (packetSize == 0) return; + lora_last_rx = millis(); - LoRa.readBytes((uint8_t *)&incoming, packetSize); + lora_last_rx -= (stats.last_tx_duration > 0 ) ? stats.last_tx_duration : 0; // RX time should be the same as TX time + sys_rssi = LoRa.packetRssi(); sys_ppsc++; + LoRa.readBytes((uint8_t *)&air_0, packetSize); - uint8_t id = incoming.id - 1; + uint8_t id = air_0.id - 1; + last_received_id = air_0.id; + peers[id].id = last_received_id; + peers[id].lq_tick++; + peers[id].updated = lora_last_rx; + peers[id].rssi = sys_rssi; + if (air_0.type == 1) { // Type 1 packet (Speed + host + state + broadcast + name) - peers[id].id = incoming.id; - last_received_id = incoming.id; + air_r = (air_type1_t*)&air_0; - peers[id].host = incoming.host; - peers[id].updated = lora_last_rx; - peers[id].rssi = sys_rssi; + peers[id].host = (*air_r).host; + peers[id].state = (*air_r).state; + peers[id].broadcast = (*air_r).broadcast; + peers[id].gps.groundSpeed = (*air_r).speed * 100; + strncpy(peers[id].name, (*air_r).name, LORA_NAME_LENGTH); + peers[id].name[LORA_NAME_LENGTH] = 0; - peers[id].gps.lat = incoming.lat; - peers[id].gps.lon = incoming.lon; - peers[id].gps.alt = incoming.alt; - peers[id].gps.groundCourse = incoming.heading; - peers[id].gps.groundSpeed = incoming.speed; + } + else { // Type 0 packet (GPS + heading) + + peers[id].gps.lat = air_0.lat * 100; + peers[id].gps.lon = air_0.lon * 100; + peers[id].gps.alt = air_0.alt * 100; + peers[id].gps.groundCourse = air_0.heading * 10; + } - peers[id].tick = incoming.tick; - strncpy(peers[id].name, incoming.name, LORA_NAME_LENGTH); - peers[id].name[LORA_NAME_LENGTH] = 0; - stats.timer_end = millis(); - stats.last_rx_duration = stats.timer_end - lora_last_rx; + sys_num_peers = count_peers(); - if (incoming.id == curr.id) { // Same slot, conflict - uint32_t cs1 = incoming.name[0] + incoming.name[1] * 26 + incoming.name[2] * 26 * 26 ; - uint32_t cs2 = curr.name[0] + curr.name[1] * 26 + curr.name[2] * 26 * 26; + if ((last_received_id == curr.id) && (main_mode > MODE_LORA_SYNC)) { // Same slot, conflict + uint32_t cs1 = peers[id].name[0] + peers[id].name[1] * 26 + peers[id].name[2] * 26 * 26 ; + uint32_t cs2 = curr.name[0] + curr.name[1] * 26 + curr.name[2] * 26 * 26; if (cs1 < cs2) { // Pick another slot sprintf(sys_message, "%s", "ID CONFLICT"); pick_id(); @@ -225,31 +274,32 @@ void display_draw() { display.setFont(ArialMT_Plain_24); display.setTextAlignment(TEXT_ALIGN_RIGHT); - display.drawString(26, 0, String(curr.gps.numSat)); + display.drawString(26, 11, String(curr.gps.numSat)); display.drawString(13, 42, String(sys_num_peers)); // display.drawString(119, 0, String((float)curr.vbat / 10)); display.setFont(ArialMT_Plain_10); - display.drawString (126, 23, "_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ "); - display.drawString(107, 35, String(sys_pps)); - display.drawString(65, 45, String(sys_rssi)); - display.drawString (107, 45, String(stats.percent_received)); - display.drawString (80, 35, String(peerout.tick)); + display.drawString (126, 29, "_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ "); + display.drawString(60, 44, String(sys_pps)); + display.drawString (107, 44, String(stats.percent_received)); + display.drawString(107, 54, String(sys_rssi)); + display.setTextAlignment (TEXT_ALIGN_CENTER); + display.drawString (64, 0, String(sys_message)); + display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (60, 00, String(sys_message)); - display.drawString (60, 21, String(curr.name)); - display.drawString (27, 12, "SAT"); - display.drawString (108, 45, "%E"); - display.drawString (15, 45, "ID"); - display.drawString (28, 45, String(curr.id)); - display.drawString (109, 35, "PS"); - display.drawString (66, 45, "dB"); - display.drawString (0, 21, String(host_name[curr.host])); + display.drawString (60, 23, String(curr.name)); + display.drawString (27, 23, "SAT"); + display.drawString (108, 44, "%E"); + display.drawString (15, 44, "ID"); + display.drawString (28, 44, String(curr.id)); + display.drawString (62, 44, "PS"); + display.drawString (109, 54, "dB"); + display.drawString (60, 12, String(host_name[curr.host])); // display.drawString (120,12, "V"); if (last_received_id > 0) { - display.drawString (48 + last_received_id * 8, 54, String(last_received_id)); + display.drawString (36 + last_received_id * 8, 54, String(last_received_id)); } if (sys_num_peers == 0) { @@ -262,8 +312,8 @@ void display_draw() { display.drawString (15, 54, "Peers"); } - if (curr.gps.fixType == 1) display.drawString (30,3, "2D"); - if (curr.gps.fixType == 2) display.drawString (30,3, "3D"); + if (curr.gps.fixType == 1) display.drawString (27, 12, "2D"); + if (curr.gps.fixType == 2) display.drawString (27, 12, "3D"); } else if (sys_display_page == 1) { @@ -275,16 +325,19 @@ void display_draw() { display.drawString(0, 10, "GPS LAT"); display.drawString(0, 20, "GPS LON"); display.drawString(0, 30, "GPS ALT"); - display.drawString(0, 40, "TICK"); + display.drawString(0, 40, "HEADING"); + display.drawString(0, 50, "STATE"); + display.drawString (32, 0, String(curr.name)); - display.drawString (96, 0, String(host_name[curr.host])); display.setTextAlignment(TEXT_ALIGN_RIGHT); - display.drawString (120, 10, String((float)curr.gps.lat/10000000, 6)); - display.drawString (120, 20, String((float)curr.gps.lon/10000000, 6)); - display.drawString (120, 30, String((float)curr.gps.alt)); - display.drawString (120, 40, String(peerout.tick)); - + display.drawString (128, 0, String(host_name[curr.host])); + display.drawString (128, 10, String((float)curr.gps.lat/10000000, 6)); + display.drawString (128, 20, String((float)curr.gps.lon/10000000, 6)); + display.drawString (128, 30, String((float)curr.gps.alt)); + display.drawString (128, 40, String((float)curr.gps.groundCourse / 10)); + display.drawString (128, 50, String(host_state[curr.state])); + } else if (sys_display_page == 2) { @@ -293,17 +346,17 @@ void display_draw() { display.setTextAlignment (TEXT_ALIGN_LEFT); int j = 0; - for (int i = 0; i < LORA_MAXPEERS ; i++) { + for (int i = 0; i < LORA_NODES ; i++) { if (peers[i].id > 0 && j < 3) { line = j * 20; display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString (0, line, String(peers[i].id) + ":"); display.drawString (13, line, String(peers[i].name)); - display.drawString (50, line, String(host_name[peers[i].host])); - display.drawString (13, line + 10, "LQ " + String(peers[i].rssi)); - display.drawString (50, line + 10, String(peers[i].tick)); -// display.drawString (13, line + 20, "S:" + String(peers[i].gps.groundSpeed)); -// display.drawString (80, line + 20, "H:" + String(peers[i].gps.groundCourse / 10)); +// display.drawString (50, line, String(host_name[peers[i].host])); + display.drawString (50, line, "LQ" + String(peers[i].lq)); + display.drawString (13, line + 10, String(peers[i].rssi) + "db"); + display.drawString (50, line + 10, String(peers[i].gps.groundCourse) + "°"); +// display.drawString (13, line + 20, "S:" + String(peers[i].gps.groundSpeed)); display.setTextAlignment (TEXT_ALIGN_RIGHT); display.drawString (128, line, String((float)peers[i].gps.lat/10000000,5)); @@ -328,11 +381,11 @@ void display_draw() { display.drawString (52, 14 , String(cfg.lora_cycle / 2)); display.drawString (120, 14 , "0"); - long pos[LORA_MAXPEERS]; + long pos[LORA_NODES]; now = millis(); long diff; - for (int i = 0; i < LORA_MAXPEERS ; i++) { + for (int i = 0; i < LORA_NODES ; i++) { if (peers[i].id != 0) { diff = now - peers[i].updated; if ( diff < cfg.lora_cycle) { @@ -349,7 +402,7 @@ void display_draw() { display.drawString (120 - 120 * diff / cfg.lora_cycle, 0, String(curr.id)); } - for (int i = 0; i < LORA_MAXPEERS ; i++) { + for (int i = 0; i < LORA_NODES ; i++) { if (pos[i] > 0) { display.drawString (pos[i], 0, String(peers[i].id)); } @@ -377,7 +430,7 @@ void display_draw() { } else if (sys_display_page == 4) { stats.last_rx_duration = cfg.lora_cycle - stats.last_tx_duration - stats.last_msp_tx_duration - stats.last_msp_rx_duration - stats.last_oled_duration * cfg.lora_cycle / cfg.cycle_display; - + display.setFont (ArialMT_Plain_10); display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawString(0, 0, "TX TIME"); @@ -386,14 +439,14 @@ void display_draw() { display.drawString(0, 30, "OLED TIME"); display.drawString(0, 40, "RX+IDLE TIME"); display.drawString(0, 50, "LORA CYCLE"); - + display.drawString(112, 0, "ms"); display.drawString(112, 10, "ms"); display.drawString(112, 20, "ms"); display.drawString(112, 30, "ms"); display.drawString(112, 40, "ms"); display.drawString(112, 50, "ms"); - + display.setTextAlignment(TEXT_ALIGN_RIGHT); display.drawString (111, 0, String(stats.last_tx_duration)); display.drawString (111, 10, String(stats.last_msp_tx_duration)); @@ -437,16 +490,16 @@ void msp_set_fc() { void msp_send_peers() { msp_radar_pos_t radarPos; - for (int i = 0; i < LORA_MAXPEERS; i++) { + for (int i = 0; i < LORA_NODES; i++) { if (peers[i].id > 0) { radarPos.id = i; radarPos.state = peers[i].state; radarPos.lat = peers[i].gps.lat; radarPos.lon = peers[i].gps.lon; radarPos.alt = peers[i].gps.alt; - radarPos.heading = peers[i].gps.groundSpeed; - radarPos.speed = peers[i].gps.groundCourse; - radarPos.tick = peers[i].tick; + radarPos.heading = peers[i].gps.groundCourse; + radarPos.speed = peers[i].gps.groundSpeed; + radarPos.lq = peers[i].lq; msp.command(MSP_SET_RADAR_POS, &radarPos, sizeof(radarPos)); } } @@ -483,7 +536,7 @@ void IRAM_ATTR handleInterrupt() { void setup() { - set_mode(0); + set_mode(LORA_PERF_MODE); display_init(); @@ -504,9 +557,7 @@ void setup() { display.drawString (90, 18, "OK"); display.display(); - for (int i = 0; i < LORA_MAXPEERS; i++) { - peers[i].id = 0; - } + reset_peers(); pinMode(interruptPin, INPUT); io_button_pressed = 0; @@ -542,14 +593,12 @@ void loop() { msp_set_name(); } else { - for (int i = 0; i < LORA_NAME_LENGTH; i++) { + for (int i = 0; i < 4; i++) { curr.name[i] = (char) random(65, 90); - curr.name[LORA_NAME_LENGTH] = 0; + curr.name[4] = 0; } } - strncpy(peerout.name, curr.name, LORA_NAME_LENGTH); - peerout.host = curr.host; curr.gps.fixType = 0; curr.gps.lat = 0; curr.gps.lon = 0; @@ -587,7 +636,7 @@ void loop() { main_mode = MODE_LORA_SYNC; } else { // Still scanning if ((millis() > display_updated + cfg.cycle_display / 2) && display_enabled) { - for (int i = 0; i < LORA_MAXPEERS; i++) { + for (int i = 0; i < LORA_NODES; i++) { if (peers[i].id > 0) { display.drawString(40 + peers[i].id * 8, 38, String(peers[i].id)); } @@ -620,7 +669,7 @@ void loop() { stats.packets_total = 0; stats.packets_received = 0; stats.percent_received = 0; - peerout.tick = 0; +// peerout.tick = 0; main_mode = MODE_LORA_RX; } @@ -640,42 +689,39 @@ void loop() { if (main_mode == MODE_LORA_TX) { - if ((curr.host != HOST_NONE) && (curr.gps.fixType > 0)) { - peerout.lat = curr.gps.lat; - peerout.lon = curr.gps.lon; - peerout.alt = curr.gps.alt; - peerout.speed = curr.gps.groundSpeed; - peerout.heading = curr.gps.groundCourse / 10; - } - else { - peerout.lat = 0; - peerout.lon = 0; - peerout.alt = 0; - peerout.speed = 0; - peerout.heading = 0; + if ((curr.host == HOST_NONE) || (curr.gps.fixType < 1)) { + curr.gps.lat = 0; + curr.gps.lon = 0; + curr.gps.alt = 0; + curr.gps.groundCourse = 0; + curr.gps.groundSpeed = 0; } - peerout.tick++; + // peerout.tick++; - stats.last_tx_begin = millis(); - lora_send(&peerout); + // stats.last_tx_begin = millis(); lora_last_tx = millis(); - stats.last_tx_duration = lora_last_tx - stats.last_tx_begin; + lora_send(); + stats.timer_end = millis(); + stats.last_tx_duration = stats.timer_end - lora_last_tx; // Drift correction - if ((curr.id > 1) && (peers[curr.id - 1].id > 0)) { - lora_drift = lora_last_tx - peers[curr.id - 1].updated - cfg.lora_slot_spacing; + if (curr.id > 1) { + int prev = curr.id - 2; + if (peers[prev].id > 0) { + lora_drift = lora_last_tx - peers[prev].updated - cfg.lora_slot_spacing; - if ((abs(lora_drift) > cfg.lora_antidrift_threshold) && (abs(lora_drift) < (cfg.lora_slot_spacing * 0.5))) { - drift_correction = constrain(lora_drift, -cfg.lora_antidrift_correction, cfg.lora_antidrift_correction); - lora_next_tx += drift_correction; - sprintf(sys_message, "%s %3d", "DRIFT:", drift_correction); + if ((abs(lora_drift) > cfg.lora_antidrift_threshold) && (abs(lora_drift) < (cfg.lora_slot_spacing * 0.5))) { + drift_correction = constrain(lora_drift, -cfg.lora_antidrift_correction, cfg.lora_antidrift_correction); + lora_next_tx -= drift_correction; + sprintf(sys_message, "%s %3d", "TIMING ADJUST", -drift_correction); + } } } msp_step = 0; - msp_next_cycle = stats.last_tx_begin + cfg.lora_slot_spacing * 2.5; + msp_next_cycle = stats.last_tx_begin + cfg.lora_slot_spacing + cfg.msp_after_tx_delay; // Back to RX @@ -702,18 +748,18 @@ void loop() { switch (msp_step) { - case 0: + case 0: msp_set_state(); break; - case 1: + case 1: stats.timer_begin = millis(); msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); stats.timer_end = millis(); stats.last_msp_rx_duration = stats.timer_end - stats.timer_begin; break; - case 2: + case 2: stats.timer_begin = millis(); msp_send_peers(); stats.timer_end = millis(); @@ -722,7 +768,7 @@ void loop() { } - msp_next_cycle += cfg.msp_cycle_delay; + msp_next_cycle += cfg.lora_slot_spacing; msp_step++; } @@ -735,14 +781,23 @@ void loop() { sys_ppsc = 0; now = millis(); - // Pruning the timed-out peers + // Pruning the timed-out peers + LQ computation - for (int i = 0; i < LORA_MAXPEERS; i++) { + for (int i = 0; i < LORA_NODES; i++) { + + if (now > (peers[i].lq_updated + cfg.lora_cycle * 4)) { + uint16_t diff = peers[i].updated - peers[i].lq_updated; + peers[i].lq = constrain(peers[i].lq_tick * 4.4 * cfg.lora_cycle / diff, 0, 4); + peers[i].lq_updated = now; + peers[i].lq_tick = 0; + } + if (peers[i].id > 0 && ((now - peers[i].updated) > cfg.lora_peer_timeout)) { peers[i].state = 0; peers[i].id = 0; sys_rssi = 0; } + } sys_num_peers = count_peers(); diff --git a/src/main.h b/src/main.h index 5fadbec..a462ab7 100644 --- a/src/main.h +++ b/src/main.h @@ -6,12 +6,13 @@ #define MODE_LORA_RX 3 #define MODE_LORA_TX 4 -#define LORA_NAME_LENGTH 4 +#define LORA_NAME_LENGTH 7 #define SERIAL_PIN_TX 23 #define SERIAL_PIN_RX 17 -#define LORA_MAXPEERS 5 +#define LORA_NODES 4 +#define LORA_PERF_MODE 0 #define SCK 5 // GPIO5 - SX1278's SCK #define MISO 19 // GPIO19 - SX1278's MISO @@ -24,17 +25,23 @@ #define HOST_INAV 1 #define HOST_BTFL 2 + + char host_name[3][5]={"NoFC", "iNav", "Beta"}; +char host_state[3][5]={"IDLE", "1", "2"}; struct peer_t { uint8_t id; - uint8_t state; - char name[LORA_NAME_LENGTH + 1]; uint8_t host; - uint8_t tick; + uint8_t state; + uint8_t broadcast; uint32_t updated; + uint32_t lq_updated; + uint8_t lq_tick; + uint8_t lq; int rssi; msp_raw_gps_t gps; + char name[LORA_NAME_LENGTH + 1]; }; struct curr_t { @@ -47,18 +54,26 @@ struct curr_t { msp_analog_t vbat; }; -struct peer_air_t { - uint8_t id; - uint8_t host; - uint8_t tick; - int32_t lat; - int32_t lon; - int16_t alt; - int16_t speed; - int16_t heading; - char name[LORA_NAME_LENGTH]; + +struct air_type0_t { // 80 bits + unsigned int id : 3; + unsigned int type : 3; + signed int lat : 25; // -9 000 000 to +9 000 000 + signed int lon : 26; // -18 000 000 to +18 000 000 + signed int alt : 14; // -8192m to +8192m + unsigned int heading : 9; }; +struct air_type1_t { // 80 bits + unsigned int id : 3; + unsigned int type : 3; + unsigned int host : 3; + unsigned int state : 3; + unsigned int broadcast : 6; + unsigned int speed : 6; // 64m/s + char name[LORA_NAME_LENGTH]; // 7x8 + }; + struct stats_t { uint32_t timer_begin; uint32_t timer_end; @@ -86,10 +101,11 @@ struct config_t { uint8_t lora_antidrift_correction; uint16_t lora_peer_timeout; - uint16_t msp_cycle_delay; uint16_t msp_fc_timeout; - + uint16_t msp_after_tx_delay; + uint16_t cycle_scan; uint16_t cycle_display; uint16_t cycle_stats; }; + From 6fc47ededaa43252e268c7c65a2a5e4fe7d4e605 Mon Sep 17 00:00:00 2001 From: Olivier C Date: Thu, 25 Apr 2019 21:48:58 +0200 Subject: [PATCH 13/17] v0.92, cosmetic tweaks + keeps last known GPS --- src/lib/LoRa.cpp | 13 +++- src/lib/LoRa.h | 1 + src/main.cpp | 154 ++++++++++++++++++++++++++--------------------- src/main.h | 55 +++++++++++++++-- 4 files changed, 149 insertions(+), 74 deletions(-) diff --git a/src/lib/LoRa.cpp b/src/lib/LoRa.cpp index 9bbc932..69a10b7 100644 --- a/src/lib/LoRa.cpp +++ b/src/lib/LoRa.cpp @@ -478,6 +478,7 @@ long LoRaClass::getSignalBandwidth() return -1; } +// void LoRaClass::setSignalBandwidth(long sbw, long frequency) void LoRaClass::setSignalBandwidth(long sbw) { int bw; @@ -500,8 +501,18 @@ void LoRaClass::setSignalBandwidth(long sbw) bw = 7; } else if (sbw <= 250E3) { bw = 8; - } else /*if (sbw <= 250E3)*/ { + } else /*if (sbw == 500E3)*/ { bw = 9; +/* + if (frequency == 443E6) { + writeRegister(0x36, 0x02); + writeRegister(0x3a, 0x64); + } + else { // 868E6 915E6 + writeRegister(0x36, 0x02); + writeRegister(0x3a, 0x7F); + } + */ } writeRegister(REG_MODEM_CONFIG_1, (readRegister(REG_MODEM_CONFIG_1) & 0x0f) | (bw << 4)); diff --git a/src/lib/LoRa.h b/src/lib/LoRa.h index d93f159..4c9a746 100644 --- a/src/lib/LoRa.h +++ b/src/lib/LoRa.h @@ -60,6 +60,7 @@ class LoRaClass : public Stream { void setTxPower(int level, int outputPin = PA_OUTPUT_PA_BOOST_PIN); void setFrequency(long frequency); void setSpreadingFactor(int sf); +// void setSignalBandwidth(long sbw, long frequency); void setSignalBandwidth(long sbw); void setCodingRate4(int denominator); void setPreambleLength(long length); diff --git a/src/main.cpp b/src/main.cpp index 624b0a9..11f8cb2 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -63,7 +63,7 @@ void set_mode(uint8_t mode) { switch (mode) { - case 0 : + case 0 : // SF9 250 cfg.lora_frequency = 433E6; // 433E6, 868E6, 915E6 cfg.lora_bandwidth = 250000; cfg.lora_coding_rate = 5; @@ -74,16 +74,17 @@ void set_mode(uint8_t mode) { cfg.lora_timing_delay = -60; cfg.lora_antidrift_threshold = 5; cfg.lora_antidrift_correction = 5; - cfg.lora_peer_timeout = 5000; + cfg.lora_peer_timeout = 6000; cfg.msp_fc_timeout = 7000; cfg.msp_after_tx_delay = 85; - - cfg.cycle_scan = 3000; - cfg.cycle_display = 125; + + cfg.cycle_scan = 2500; + cfg.cycle_display = 250; cfg.cycle_stats = 1000; break; + } } @@ -142,7 +143,7 @@ void lora_send() { air_1.id = curr.id; air_1.type = 1; air_1.host = curr.host; - air_1.state = 0; + air_1.state = curr.state; air_1.broadcast = 0; air_1.speed = curr.gps.groundSpeed / 100; strncpy(air_1.name, curr.name, LORA_NAME_LENGTH); @@ -162,7 +163,7 @@ void lora_send() { while (!LoRa.beginPacket()) { } LoRa.write((uint8_t*)&air_0, sizeof(air_0)); LoRa.endPacket(false); - } + } } void lora_receive(int packetSize) { @@ -202,9 +203,15 @@ void lora_receive(int packetSize) { peers[id].gps.lon = air_0.lon * 100; peers[id].gps.alt = air_0.alt * 100; peers[id].gps.groundCourse = air_0.heading * 10; - } - + if (peers[id].gps.lat != 0 && peers[id].gps.lon != 0) { // Save the last known coordinates + peers[id].gpsrec.lat = peers[id].gps.lat; + peers[id].gpsrec.lon = peers[id].gps.lon; + peers[id].gpsrec.alt = peers[id].gps.alt; + peers[id].gpsrec.groundCourse = peers[id].gps.groundCourse; + peers[id].gpsrec.groundSpeed = peers[id].gps.groundSpeed; + } + } sys_num_peers = count_peers(); @@ -230,6 +237,7 @@ void lora_init() { } LoRa.sleep(); +// LoRa.setSignalBandwidth(cfg.lora_bandwidth, cfg.lora_frequency); LoRa.setSignalBandwidth(cfg.lora_bandwidth); LoRa.setCodingRate4(cfg.lora_coding_rate); LoRa.setSpreadingFactor(cfg.lora_spreading_factor); @@ -254,16 +262,6 @@ void display_init() { display.setTextAlignment(TEXT_ALIGN_LEFT); } -void display_disable() { - display.displayOff(); - display_enabled = 0; -} - -void display_enable() { - display.displayOn(); - display_enabled = 1; -} - void display_draw() { display.clear(); @@ -276,30 +274,31 @@ void display_draw() { display.setTextAlignment(TEXT_ALIGN_RIGHT); display.drawString(26, 11, String(curr.gps.numSat)); display.drawString(13, 42, String(sys_num_peers)); + display.drawString (125, 11, String(peer_slotname[curr.id])); + // display.drawString(119, 0, String((float)curr.vbat / 10)); display.setFont(ArialMT_Plain_10); display.drawString (126, 29, "_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ "); - display.drawString(60, 44, String(sys_pps)); display.drawString (107, 44, String(stats.percent_received)); display.drawString(107, 54, String(sys_rssi)); display.setTextAlignment (TEXT_ALIGN_CENTER); display.drawString (64, 0, String(sys_message)); - + display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (60, 23, String(curr.name)); + display.drawString (60, 12, String(curr.name)); display.drawString (27, 23, "SAT"); display.drawString (108, 44, "%E"); - display.drawString (15, 44, "ID"); - display.drawString (28, 44, String(curr.id)); - display.drawString (62, 44, "PS"); +// display.drawString (15, 44, "ID"); + + display.drawString(23, 44, String(sys_pps) + "p/s"); display.drawString (109, 54, "dB"); - display.drawString (60, 12, String(host_name[curr.host])); + display.drawString (60, 23, String(host_name[curr.host])); // display.drawString (120,12, "V"); if (last_received_id > 0) { - display.drawString (36 + last_received_id * 8, 54, String(last_received_id)); + display.drawString (36 + last_received_id * 8, 54, String(peer_slotname[last_received_id])); } if (sys_num_peers == 0) { @@ -327,7 +326,7 @@ void display_draw() { display.drawString(0, 30, "GPS ALT"); display.drawString(0, 40, "HEADING"); display.drawString(0, 50, "STATE"); - + display.drawString (32, 0, String(curr.name)); display.setTextAlignment(TEXT_ALIGN_RIGHT); @@ -337,7 +336,7 @@ void display_draw() { display.drawString (128, 30, String((float)curr.gps.alt)); display.drawString (128, 40, String((float)curr.gps.groundCourse / 10)); display.drawString (128, 50, String(host_state[curr.state])); - + } else if (sys_display_page == 2) { @@ -350,17 +349,28 @@ void display_draw() { if (peers[i].id > 0 && j < 3) { line = j * 20; display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (0, line, String(peers[i].id) + ":"); - display.drawString (13, line, String(peers[i].name)); -// display.drawString (50, line, String(host_name[peers[i].host])); - display.drawString (50, line, "LQ" + String(peers[i].lq)); - display.drawString (13, line + 10, String(peers[i].rssi) + "db"); - display.drawString (50, line + 10, String(peers[i].gps.groundCourse) + "°"); -// display.drawString (13, line + 20, "S:" + String(peers[i].gps.groundSpeed)); + display.setFont(ArialMT_Plain_16); + display.drawString (0, line, String(peer_slotname[peers[i].id])); + + display.setFont(ArialMT_Plain_10); + display.drawString (30, line, String(peers[i].name)); + + if (peers[i].state == 2) { display.drawString (14, line, "L"); } + else if (peers[i].lq == 0) { display.drawString (14, line, "x"); } + else if (peers[i].lq == 1) { display.drawXbm(16, line + 3, 8, 8, icon_lq_1); } + else if (peers[i].lq == 2) { display.drawXbm(16, line + 3, 8, 8, icon_lq_2); } + else if (peers[i].lq == 3) { display.drawXbm(16, line + 3, 8, 8, icon_lq_3); } + else if (peers[i].lq == 4) { display.drawXbm(16, line + 3, 8, 8, icon_lq_4); } + +// display.drawString (48, line, String(host_name[peers[i].host])); + display.drawString (16, line + 10, String(peers[i].rssi) + "db"); +// display.drawString (50, line + 10, String(peers[i].gpsrec.groundCourse) + "°"); + display.drawString (53, line + 10, String(peers[i].gpsrec.alt)); +// display.drawString (13, line + 20, "S:" + String(peers[i].gpsrec.groundSpeed)); display.setTextAlignment (TEXT_ALIGN_RIGHT); - display.drawString (128, line, String((float)peers[i].gps.lat/10000000,5)); - display.drawString (128, line + 10, String((float)peers[i].gps.lon/10000000,5)); + display.drawString (128, line, String((float)peers[i].gpsrec.lat / 10000000, 5)); + display.drawString (128, line + 10, String((float)peers[i].gpsrec.lon / 10000000, 5)); j++; } @@ -373,13 +383,13 @@ void display_draw() { display.setFont (ArialMT_Plain_10); display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (0, 1 , "_____________________"); - display.drawString (3, 4 , "."); - display.drawString (60, 4 , "."); - display.drawString (122, 4 , "."); - display.drawString (0, 14 , String(cfg.lora_cycle)); - display.drawString (52, 14 , String(cfg.lora_cycle / 2)); - display.drawString (120, 14 , "0"); + display.drawHorizontalLine(0, 11, 128); + display.drawVerticalLine(0, 9, 6); + display.drawVerticalLine(64, 11, 4); + display.drawVerticalLine(127, 9, 6); + display.drawString (2, 12 , String(cfg.lora_cycle)); +// display.drawString (54, 14 , String(cfg.lora_cycle / 2)); + display.drawString (119, 12 , "0"); long pos[LORA_NODES]; now = millis(); @@ -399,29 +409,35 @@ void display_draw() { diff = now - lora_last_tx; if ( diff < cfg.lora_cycle) { - display.drawString (120 - 120 * diff / cfg.lora_cycle, 0, String(curr.id)); + display.drawString (120 - 120 * diff / cfg.lora_cycle, 0, String(peer_slotname[curr.id])); } for (int i = 0; i < LORA_NODES ; i++) { if (pos[i] > 0) { - display.drawString (pos[i], 0, String(peers[i].id)); + display.drawString (pos[i], 0, String(peer_slotname[peers[i].id])); } if (peers[i].id > 0 && j < 4) { line = j * 9 + 24; display.setTextAlignment (TEXT_ALIGN_LEFT); - display.drawString (0, line, String(peers[i].id) + ":"); + display.drawString (0, line, String(peer_slotname[peers[i].id])); display.drawString (16, line, String(peers[i].name)); display.drawString (56, line, String(host_name[peers[i].host])); display.setTextAlignment (TEXT_ALIGN_RIGHT); - if (lora_last_tx > peers[i].updated) { - display.drawString (120, line, String(lora_last_tx - peers[i].updated)); - display.drawString (128, line, "-"); + if (peers[i].state == 2) { // Peer timed out (L) + display.drawString (127, line, "L:" + String((int)((lora_last_tx - peers[i].updated) / 1000)) + "s" ); } else { - display.drawString (120, line, String((peers[i].updated) - lora_last_tx)); - display.drawString (128, line, "+"); + if (lora_last_tx > peers[i].updated) { + display.drawString (119, line, String(lora_last_tx - peers[i].updated)); + display.drawString (127, line, "-"); + } + else { + display.drawString (119, line, String((peers[i].updated) - lora_last_tx)); + display.drawString (127, line, "+"); + + } } j++; } @@ -638,7 +654,7 @@ void loop() { if ((millis() > display_updated + cfg.cycle_display / 2) && display_enabled) { for (int i = 0; i < LORA_NODES; i++) { if (peers[i].id > 0) { - display.drawString(40 + peers[i].id * 8, 38, String(peers[i].id)); + display.drawString(40 + peers[i].id * 8, 38, String(peer_slotname[peers[i].id])); } } display.drawProgressBar(0, 53, 126, 6, 100 * (millis() - cycle_scan_begin) / cfg.cycle_scan); @@ -669,7 +685,7 @@ void loop() { stats.packets_total = 0; stats.packets_received = 0; stats.percent_received = 0; -// peerout.tick = 0; + stats.up_time_begin = millis(); main_mode = MODE_LORA_RX; } @@ -748,18 +764,18 @@ void loop() { switch (msp_step) { - case 0: + case 0: msp_set_state(); break; - case 1: + case 1: stats.timer_begin = millis(); msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); stats.timer_end = millis(); stats.last_msp_rx_duration = stats.timer_end - stats.timer_begin; break; - case 2: + case 2: stats.timer_begin = millis(); msp_send_peers(); stats.timer_end = millis(); @@ -781,20 +797,20 @@ void loop() { sys_ppsc = 0; now = millis(); - // Pruning the timed-out peers + LQ computation + // Pausing the timed-out peers + LQ computation for (int i = 0; i < LORA_NODES; i++) { - + if (now > (peers[i].lq_updated + cfg.lora_cycle * 4)) { uint16_t diff = peers[i].updated - peers[i].lq_updated; peers[i].lq = constrain(peers[i].lq_tick * 4.4 * cfg.lora_cycle / diff, 0, 4); peers[i].lq_updated = now; peers[i].lq_tick = 0; } - + if (peers[i].id > 0 && ((now - peers[i].updated) > cfg.lora_peer_timeout)) { - peers[i].state = 0; - peers[i].id = 0; + peers[i].state = 2; + // peers[i].id = 0; sys_rssi = 0; } @@ -807,15 +823,17 @@ void loop() { // Screen management - if ((curr.state == 0) && (display_enabled == 0)) { // Aircraft is disarmed = Turning on the OLED - display_enable(); + if (!curr.state && !display_enabled) { // Aircraft is disarmed = Turning on the OLED + display.displayOn(); + display_enabled = 1; } - else if ((curr.state == 1) && (display_enabled == 1)) { // Aircraft is armed = Turning off the OLED - display_disable(); + else if (curr.state && display_enabled) { // Aircraft is armed = Turning off the OLED + display.displayOff(); + display_enabled = 0; } - stats_updated = millis(); + stats_updated = now; } } diff --git a/src/main.h b/src/main.h index a462ab7..4408e66 100644 --- a/src/main.h +++ b/src/main.h @@ -1,4 +1,4 @@ -#define VERSION "0.91" +#define VERSION "0.92" #define MODE_HOST_SCAN 0 #define MODE_LORA_INIT 1 @@ -25,10 +25,9 @@ #define HOST_INAV 1 #define HOST_BTFL 2 - - char host_name[3][5]={"NoFC", "iNav", "Beta"}; -char host_state[3][5]={"IDLE", "1", "2"}; +char host_state[3][5]={"IDLE", "ARM", ""}; +char peer_slotname[9][3]={"", "A", "B", "C", "D", "E", "F", "G", "H"}; struct peer_t { uint8_t id; @@ -41,6 +40,7 @@ struct peer_t { uint8_t lq; int rssi; msp_raw_gps_t gps; + msp_raw_gps_t gpsrec; char name[LORA_NAME_LENGTH + 1]; }; @@ -54,7 +54,6 @@ struct curr_t { msp_analog_t vbat; }; - struct air_type0_t { // 80 bits unsigned int id : 3; unsigned int type : 3; @@ -75,6 +74,7 @@ struct air_type1_t { // 80 bits }; struct stats_t { + uint32_t up_time_begin; uint32_t timer_begin; uint32_t timer_end; float packets_total; @@ -109,3 +109,48 @@ struct config_t { uint16_t cycle_stats; }; +const uint8_t icon_lq_1[] PROGMEM = { + B00000000, + B00000000, + B00000000, + B00000000, + B00000000, + B00000000, + B00000000, + B00000011 +}; + +const uint8_t icon_lq_2[] PROGMEM = { + B00000000, + B00000000, + B00000000, + B00000000, + B00000000, + B00001111, + B00000000, + B00000011 +}; + +const uint8_t icon_lq_3[] PROGMEM = { + B00000000, + B00000000, + B00000000, + B00111111, + B00000000, + B00001111, + B00000000, + B00000011 +}; + +const uint8_t icon_lq_4[] PROGMEM = { + B00000000, + B11111111, + B00000000, + B00111111, + B00000000, + B00001111, + B00000000, + B00000011 +}; + + From f762f30e327eab74691b7891cde3225615db43f3 Mon Sep 17 00:00:00 2001 From: Olivier C Date: Fri, 26 Apr 2019 17:45:52 +0200 Subject: [PATCH 14/17] Added silent mode if all nodes taken --- src/main.cpp | 34 +++++++++++++++++++++++++--------- src/main.h | 2 +- 2 files changed, 26 insertions(+), 10 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 11f8cb2..ab7f8a9 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -23,6 +23,7 @@ air_type1_t * air_r; stats_t stats; int sys_rssi; uint8_t main_mode; +bool silent_mode = 0; uint8_t last_received_id = 0; char sys_message[20]; @@ -634,9 +635,9 @@ void loop() { } else { // Still scanning if ((millis() > display_updated + cfg.cycle_display / 2) && display_enabled) { - delay(cfg.cycle_display / 2); + delay(100); msp_set_fc(); - + display.drawProgressBar(35, 30, 48, 6, 100 * (millis() - cycle_scan_begin) / cfg.msp_fc_timeout); display.display(); display_updated = millis(); @@ -648,8 +649,17 @@ void loop() { if (main_mode == MODE_LORA_INIT) { if (millis() > (cycle_scan_begin + cfg.cycle_scan)) { // End of the scan, set the ID then sync - pick_id(); - main_mode = MODE_LORA_SYNC; + + sys_num_peers = count_peers(); + + if (sys_num_peers >= LORA_NODES) { + silent_mode = 1; + } + else { + pick_id(); + } + main_mode = MODE_LORA_SYNC; + } else { // Still scanning if ((millis() > display_updated + cfg.cycle_display / 2) && display_enabled) { for (int i = 0; i < LORA_NODES; i++) { @@ -668,9 +678,7 @@ void loop() { if (main_mode == MODE_LORA_SYNC) { - sys_num_peers = count_peers(); - - if (sys_num_peers == 0) { // Alone, start at will + if (sys_num_peers == 0 || silent_mode) { // Alone or silent mode, start at will lora_next_tx = millis() + cfg.lora_cycle; } else { // Not alone, sync by slot @@ -693,12 +701,20 @@ void loop() { // ---------------------- LORA RX if ((main_mode == MODE_LORA_RX) && (millis() > lora_next_tx)) { + now = millis(); while (now > lora_next_tx) { // In case we skipped some beats - lora_next_tx += cfg.lora_cycle; + lora_next_tx += cfg.lora_cycle; + lora_last_tx = lora_next_tx; + } + + if (silent_mode) { + sprintf(sys_message, "%s", "SILENT MODE (NO TX)"); + } + else { + main_mode = MODE_LORA_TX; } - main_mode = MODE_LORA_TX; } // ---------------------- LORA TX diff --git a/src/main.h b/src/main.h index 4408e66..5d92cf9 100644 --- a/src/main.h +++ b/src/main.h @@ -27,7 +27,7 @@ char host_name[3][5]={"NoFC", "iNav", "Beta"}; char host_state[3][5]={"IDLE", "ARM", ""}; -char peer_slotname[9][3]={"", "A", "B", "C", "D", "E", "F", "G", "H"}; +char peer_slotname[9][3]={"X", "A", "B", "C", "D", "E", "F", "G", "H"}; struct peer_t { uint8_t id; From 4ce1f4755f396648ced793d98b79c853278e0551 Mon Sep 17 00:00:00 2001 From: Olivier C Date: Fri, 26 Apr 2019 18:04:23 +0200 Subject: [PATCH 15/17] v1.0 --- src/main.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main.h b/src/main.h index 5d92cf9..a196326 100644 --- a/src/main.h +++ b/src/main.h @@ -1,4 +1,4 @@ -#define VERSION "0.92" +#define VERSION "1.0" #define MODE_HOST_SCAN 0 #define MODE_LORA_INIT 1 From 6f45c2bcf57f060fa4c76e37fdf15229e09aff16 Mon Sep 17 00:00:00 2001 From: Olivier C Date: Fri, 26 Apr 2019 18:09:30 +0200 Subject: [PATCH 16/17] Compatibility fix for Betaflight as host --- src/main.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index ab7f8a9..2478137 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -792,10 +792,12 @@ void loop() { break; case 2: - stats.timer_begin = millis(); - msp_send_peers(); - stats.timer_end = millis(); - stats.last_msp_tx_duration = stats.timer_end - stats.timer_begin; + if (curr.host == HOST_INAV) { + stats.timer_begin = millis(); + msp_send_peers(); + stats.timer_end = millis(); + stats.last_msp_tx_duration = stats.timer_end - stats.timer_begin; + } break; } From 0113ddd2f28777302ff029f713f1417353479bdc Mon Sep 17 00:00:00 2001 From: Olivier C Date: Tue, 7 May 2019 12:53:08 +0200 Subject: [PATCH 17/17] v1.01 Added vbat and mah displays. Not yet sent to other nodes --- src/main.cpp | 276 +++++++++++++++++++++++++++++++++------------------ src/main.h | 18 ++-- 2 files changed, 192 insertions(+), 102 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 2478137..a9c97fb 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,6 +6,8 @@ #include #include +#include +#include // ----------------------------------------------------------------------------- global vars @@ -41,7 +43,6 @@ uint32_t io_button_released = 0; bool io_button_pressed = 0; bool display_enabled = 1; - // Timing uint32_t cycle_scan_begin; @@ -83,7 +84,7 @@ void set_mode(uint8_t mode) { cfg.cycle_scan = 2500; cfg.cycle_display = 250; cfg.cycle_stats = 1000; - + break; } @@ -111,7 +112,10 @@ void reset_peers() { peers[i].lq = 0; peers[i].updated = 0; peers[i].rssi = 0; - strcpy(peers[i].name, "----"); + peers[i].distance = 0; + peers[i].direction = 0; + peers[i].relalt = 0; + strcpy(peers[i].name, ""); } } @@ -134,6 +138,37 @@ void resync_tx_slot(int16_t delay) { } } +// ----------------------------------------------------------------------------- calc gps distance + +double deg2rad(double deg) { + return (deg * M_PI / 180); +} + +double rad2deg(double rad) { + return (rad * 180 / M_PI); +} + +/** + * Returns the distance between two points on the Earth. + * Direct translation from http://en.wikipedia.org/wiki/Haversine_formula + * @param lat1d Latitude of the first point in degrees + * @param lon1d Longitude of the first point in degrees + * @param lat2d Latitude of the second point in degrees + * @param lon2d Longitude of the second point in degrees + * @return The distance between the two points in meters + */ + +double distanceEarth(double lat1d, double lon1d, double lat2d, double lon2d) { + double lat1r, lon1r, lat2r, lon2r, u, v; + lat1r = deg2rad(lat1d); + lon1r = deg2rad(lon1d); + lat2r = deg2rad(lat2d); + lon2r = deg2rad(lon2d); + u = sin((lat2r - lat1r)/2); + v = sin((lon2r - lon1r)/2); + return 2.0 * 6371000 * asin(sqrt(u * u + cos(lat1r) * cos(lat2r) * v * v)); +} + // -------- LoRa void lora_send() { @@ -164,7 +199,7 @@ void lora_send() { while (!LoRa.beginPacket()) { } LoRa.write((uint8_t*)&air_0, sizeof(air_0)); LoRa.endPacket(false); - } + } } void lora_receive(int packetSize) { @@ -216,7 +251,7 @@ void lora_receive(int packetSize) { sys_num_peers = count_peers(); - if ((last_received_id == curr.id) && (main_mode > MODE_LORA_SYNC)) { // Same slot, conflict + if ((last_received_id == curr.id) && (main_mode > MODE_LORA_SYNC) && !silent_mode) { // Same slot, conflict uint32_t cs1 = peers[id].name[0] + peers[id].name[1] * 26 + peers[id].name[2] * 26 * 26 ; uint32_t cs2 = curr.name[0] + curr.name[1] * 26 + curr.name[2] * 26 * 26; if (cs1 < cs2) { // Pick another slot @@ -291,12 +326,10 @@ void display_draw() { display.drawString (60, 12, String(curr.name)); display.drawString (27, 23, "SAT"); display.drawString (108, 44, "%E"); -// display.drawString (15, 44, "ID"); display.drawString(23, 44, String(sys_pps) + "p/s"); display.drawString (109, 54, "dB"); display.drawString (60, 23, String(host_name[curr.host])); -// display.drawString (120,12, "V"); if (last_received_id > 0) { display.drawString (36 + last_received_id * 8, 54, String(peer_slotname[last_received_id])); @@ -318,78 +351,13 @@ void display_draw() { else if (sys_display_page == 1) { - display.setFont (ArialMT_Plain_10); - display.setTextAlignment (TEXT_ALIGN_LEFT); - - display.drawString(0, 0, "HOST:"); - display.drawString(0, 10, "GPS LAT"); - display.drawString(0, 20, "GPS LON"); - display.drawString(0, 30, "GPS ALT"); - display.drawString(0, 40, "HEADING"); - display.drawString(0, 50, "STATE"); - - display.drawString (32, 0, String(curr.name)); - - display.setTextAlignment(TEXT_ALIGN_RIGHT); - display.drawString (128, 0, String(host_name[curr.host])); - display.drawString (128, 10, String((float)curr.gps.lat/10000000, 6)); - display.drawString (128, 20, String((float)curr.gps.lon/10000000, 6)); - display.drawString (128, 30, String((float)curr.gps.alt)); - display.drawString (128, 40, String((float)curr.gps.groundCourse / 10)); - display.drawString (128, 50, String(host_state[curr.state])); - - } - - else if (sys_display_page == 2) { - - display.setFont (ArialMT_Plain_10); - display.setTextAlignment (TEXT_ALIGN_LEFT); - - int j = 0; - for (int i = 0; i < LORA_NODES ; i++) { - if (peers[i].id > 0 && j < 3) { - line = j * 20; - display.setTextAlignment (TEXT_ALIGN_LEFT); - display.setFont(ArialMT_Plain_16); - display.drawString (0, line, String(peer_slotname[peers[i].id])); - - display.setFont(ArialMT_Plain_10); - display.drawString (30, line, String(peers[i].name)); - - if (peers[i].state == 2) { display.drawString (14, line, "L"); } - else if (peers[i].lq == 0) { display.drawString (14, line, "x"); } - else if (peers[i].lq == 1) { display.drawXbm(16, line + 3, 8, 8, icon_lq_1); } - else if (peers[i].lq == 2) { display.drawXbm(16, line + 3, 8, 8, icon_lq_2); } - else if (peers[i].lq == 3) { display.drawXbm(16, line + 3, 8, 8, icon_lq_3); } - else if (peers[i].lq == 4) { display.drawXbm(16, line + 3, 8, 8, icon_lq_4); } - -// display.drawString (48, line, String(host_name[peers[i].host])); - display.drawString (16, line + 10, String(peers[i].rssi) + "db"); -// display.drawString (50, line + 10, String(peers[i].gpsrec.groundCourse) + "°"); - display.drawString (53, line + 10, String(peers[i].gpsrec.alt)); -// display.drawString (13, line + 20, "S:" + String(peers[i].gpsrec.groundSpeed)); - - display.setTextAlignment (TEXT_ALIGN_RIGHT); - display.drawString (128, line, String((float)peers[i].gpsrec.lat / 10000000, 5)); - display.drawString (128, line + 10, String((float)peers[i].gpsrec.lon / 10000000, 5)); - - j++; - } - } - if (j == 0) { - display.drawString (0, 0, "NO PEER DETECTED"); - } - } - else if (sys_display_page == 3) { - - display.setFont (ArialMT_Plain_10); + display.setFont (ArialMT_Plain_10); display.setTextAlignment (TEXT_ALIGN_LEFT); display.drawHorizontalLine(0, 11, 128); display.drawVerticalLine(0, 9, 6); display.drawVerticalLine(64, 11, 4); display.drawVerticalLine(127, 9, 6); display.drawString (2, 12 , String(cfg.lora_cycle)); -// display.drawString (54, 14 , String(cfg.lora_cycle / 2)); display.drawString (119, 12 , "0"); long pos[LORA_NODES]; @@ -397,7 +365,7 @@ void display_draw() { long diff; for (int i = 0; i < LORA_NODES ; i++) { - if (peers[i].id != 0) { + if (peers[i].id != 0 && peers[i].state != 2) { diff = now - peers[i].updated; if ( diff < cfg.lora_cycle) { pos[i] = 120 - 120 * diff / cfg.lora_cycle; @@ -437,15 +405,17 @@ void display_draw() { else { display.drawString (119, line, String((peers[i].updated) - lora_last_tx)); display.drawString (127, line, "+"); - + } } j++; } } - } - else if (sys_display_page == 4) { + } + + else if (sys_display_page == 2) { + stats.last_rx_duration = cfg.lora_cycle - stats.last_tx_duration - stats.last_msp_tx_duration - stats.last_msp_rx_duration - stats.last_oled_duration * cfg.lora_cycle / cfg.cycle_display; display.setFont (ArialMT_Plain_10); @@ -471,7 +441,110 @@ void display_draw() { display.drawString (111, 30, String(stats.last_oled_duration)); display.drawString (111, 40, String(stats.last_rx_duration)); display.drawString (111, 50, String(cfg.lora_cycle)); + + + } + else if (sys_display_page >= 3) { + + int i = constrain(sys_display_page + 1 - LORA_NODES, 0, LORA_NODES - 1); + bool iscurrent = (i + 1 == curr.id); + + display.setFont(ArialMT_Plain_24); + display.setTextAlignment (TEXT_ALIGN_LEFT); + display.drawString (0, 0, String(peer_slotname[i + 1])); + + display.setFont(ArialMT_Plain_16); + display.setTextAlignment(TEXT_ALIGN_RIGHT); + + if (iscurrent) { + display.drawString (128, 0, String(curr.name)); + } + else { + display.drawString (128, 0, String(peers[i].name)); + } + + display.setTextAlignment (TEXT_ALIGN_LEFT); + display.setFont (ArialMT_Plain_10); + + if (peers[i].id > 0 || iscurrent) { + + if (peers[i].state == 2 && !iscurrent) { display.drawString (19, 0, "LOST"); } + else if (peers[i].lq == 0 && !iscurrent) { display.drawString (19, 0, "x"); } + else if (peers[i].lq == 1) { display.drawXbm(19, 2, 8, 8, icon_lq_1); } + else if (peers[i].lq == 2) { display.drawXbm(19, 2, 8, 8, icon_lq_2); } + else if (peers[i].lq == 3) { display.drawXbm(19, 2, 8, 8, icon_lq_3); } + else if (peers[i].lq == 4) { display.drawXbm(19, 2, 8, 8, icon_lq_4); } + + if (iscurrent) { + display.drawString (19, 0, ""); + display.drawString (19, 12, String(host_name[curr.host])); + } + else { + if (peers[i].state != 2) { + display.drawString (38, 0, String(peers[i].rssi) + "db"); + } + display.drawString (19, 12, String(host_name[peers[i].host])); + } + + if (iscurrent) { + display.drawString (50, 12, String(host_state[curr.state])); + } + else { + display.drawString (50, 12, String(host_state[peers[i].state])); + } + + display.setTextAlignment (TEXT_ALIGN_RIGHT); + + if (iscurrent) { + display.drawString (128, 24, "LA " + String((float)curr.gps.lat / 10000000, 6)); + display.drawString (128, 34, "LO "+ String((float)curr.gps.lon / 10000000, 6)); + } + else { + display.drawString (128, 24, "LA " + String((float)peers[i].gpsrec.lat / 10000000, 6)); + display.drawString (128, 34, "LO "+ String((float)peers[i].gpsrec.lon / 10000000, 6)); + } + + display.setTextAlignment (TEXT_ALIGN_LEFT); + + if (iscurrent) { + display.drawString (0, 24, "A " + String(curr.gps.alt) + "m"); + display.drawString (0, 34, "S " + String(peers[i].gpsrec.groundSpeed) + "m/s"); + display.drawString (0, 44, "C " + String(curr.gps.groundCourse / 10) + "°"); + } + else { + display.drawString (0, 24, "A " + String(peers[i].gpsrec.alt) + "m"); + display.drawString (0, 34, "S " + String(peers[i].gpsrec.groundSpeed) + "m/s"); + display.drawString (0, 44, "C " + String(peers[i].gpsrec.groundCourse / 10) + "°"); + } + + if (peers[i].gpsrec.lat != 0 && peers[i].gpsrec.lon != 0 && curr.gps.lat != 0 && curr.gps.lon != 0 && !iscurrent) { + peers[i].distance = distanceEarth(curr.gps.lat / 10000000, curr.gps.lon / 10000000, peers[i].gpsrec.lat / 10000000, peers[i].gpsrec.lon / 10000000); + peers[i].direction = 0; + peers[i].relalt = peers[i].gpsrec.alt - curr.gps.alt; + } + else { + peers[i].distance = 0; + peers[i].direction = 0; + peers[i].relalt = 0; + } + + display.drawString (40, 44, "B " + String(peers[i].direction) + "°"); + display.drawString (88, 44, "D " + String(peers[i].distance) + "m"); + display.drawString (0, 54, "R " + String(peers[i].relalt) + "m"); + if (iscurrent) { + display.drawString (40, 54, String((float)curr.fcanalog.vbat / 10) + "v"); + display.drawString (88, 54, String((int)curr.fcanalog.mAhDrawn) + "mah"); + } + + display.setTextAlignment (TEXT_ALIGN_RIGHT); + + } + else { + display.drawString (35, 7, "SLOT IS EMPTY"); + } + } + last_received_id = 0; sys_message[0] = 0; display.display(); @@ -505,6 +578,10 @@ void msp_set_fc() { } } + void msp_set_fcanalog() { + msp.request(MSP_ANALOG, &curr.fcanalog, sizeof(curr.fcanalog)); +} + void msp_send_peers() { msp_radar_pos_t radarPos; for (int i = 0; i < LORA_NODES; i++) { @@ -536,7 +613,7 @@ void IRAM_ATTR handleInterrupt() { if (io_button_pressed == 0) { io_button_pressed = 1; - if (sys_display_page >= 4) { + if (sys_display_page >= 3 + LORA_NODES) { sys_display_page = 0; } else { @@ -637,7 +714,7 @@ void loop() { delay(100); msp_set_fc(); - + display.drawProgressBar(35, 30, 48, 6, 100 * (millis() - cycle_scan_begin) / cfg.msp_fc_timeout); display.display(); display_updated = millis(); @@ -649,17 +726,18 @@ void loop() { if (main_mode == MODE_LORA_INIT) { if (millis() > (cycle_scan_begin + cfg.cycle_scan)) { // End of the scan, set the ID then sync - + sys_num_peers = count_peers(); - - if (sys_num_peers >= LORA_NODES) { + + if (sys_num_peers >= LORA_NODES || io_button_released > 0) { silent_mode = 1; + sys_display_page = 0; } else { pick_id(); } - main_mode = MODE_LORA_SYNC; - + main_mode = MODE_LORA_SYNC; + } else { // Still scanning if ((millis() > display_updated + cfg.cycle_display / 2) && display_enabled) { for (int i = 0; i < LORA_NODES; i++) { @@ -708,7 +786,7 @@ void loop() { lora_next_tx += cfg.lora_cycle; lora_last_tx = lora_next_tx; } - + if (silent_mode) { sprintf(sys_message, "%s", "SILENT MODE (NO TX)"); } @@ -753,7 +831,7 @@ void loop() { } msp_step = 0; - msp_next_cycle = stats.last_tx_begin + cfg.lora_slot_spacing + cfg.msp_after_tx_delay; + msp_next_cycle = stats.last_tx_begin + cfg.msp_after_tx_delay; // Back to RX @@ -778,25 +856,33 @@ void loop() { if (((millis() > msp_next_cycle) && (msp_step < 3) && (curr.host != HOST_NONE)) && (main_mode > MODE_LORA_SYNC)) { + stats.timer_begin = millis(); + switch (msp_step) { - case 0: + case 0: // Between TX slots 0 and 1 + + if (sys_lora_sent % 8 == 0) { + msp_set_fcanalog(); + stats.timer_end = millis(); + stats.last_msp_rx_duration = stats.timer_end - stats.timer_begin; + } + break; + + case 1: // Between TX slots 1 and 2 msp_set_state(); + stats.last_msp_rx_duration = millis()- stats.timer_begin; break; - case 1: - stats.timer_begin = millis(); + case 2: // Between TX slots 2 and 3 msp.request(MSP_RAW_GPS, &curr.gps, sizeof(curr.gps)); - stats.timer_end = millis(); - stats.last_msp_rx_duration = stats.timer_end - stats.timer_begin; + stats.last_msp_rx_duration = millis() - stats.timer_begin; break; - case 2: + case 3: // Between TX slots 3 and 0 if (curr.host == HOST_INAV) { - stats.timer_begin = millis(); msp_send_peers(); - stats.timer_end = millis(); - stats.last_msp_tx_duration = stats.timer_end - stats.timer_begin; + stats.last_msp_tx_duration = millis() - stats.timer_begin; } break; diff --git a/src/main.h b/src/main.h index a196326..5dda20d 100644 --- a/src/main.h +++ b/src/main.h @@ -1,4 +1,4 @@ -#define VERSION "1.0" +#define VERSION "1.01" #define MODE_HOST_SCAN 0 #define MODE_LORA_INIT 1 @@ -39,8 +39,12 @@ struct peer_t { uint8_t lq_tick; uint8_t lq; int rssi; + uint16_t distance; + int16_t direction; + int16_t relalt; msp_raw_gps_t gps; - msp_raw_gps_t gpsrec; + msp_raw_gps_t gpsrec; + msp_analog_t fcanalog; char name[LORA_NAME_LENGTH + 1]; }; @@ -51,12 +55,12 @@ struct curr_t { char name[16]; uint8_t tick; msp_raw_gps_t gps; - msp_analog_t vbat; + msp_analog_t fcanalog; }; struct air_type0_t { // 80 bits unsigned int id : 3; - unsigned int type : 3; + unsigned int type : 3; signed int lat : 25; // -9 000 000 to +9 000 000 signed int lon : 26; // -18 000 000 to +18 000 000 signed int alt : 14; // -8192m to +8192m @@ -65,13 +69,13 @@ struct air_type0_t { // 80 bits struct air_type1_t { // 80 bits unsigned int id : 3; - unsigned int type : 3; + unsigned int type : 3; unsigned int host : 3; unsigned int state : 3; unsigned int broadcast : 6; unsigned int speed : 6; // 64m/s char name[LORA_NAME_LENGTH]; // 7x8 - }; + }; struct stats_t { uint32_t up_time_begin; @@ -103,7 +107,7 @@ struct config_t { uint16_t msp_fc_timeout; uint16_t msp_after_tx_delay; - + uint16_t cycle_scan; uint16_t cycle_display; uint16_t cycle_stats;