From f15cb1e789115e806d0e6cfa688f46bf43e07e3b Mon Sep 17 00:00:00 2001 From: Milosz Klim Date: Mon, 13 Apr 2026 09:24:57 +0200 Subject: [PATCH 1/4] Added drone factory --- WST-FC/include/DroneFactory.h | 16 ++++++++++++ WST-FC/platformio.ini | 40 +++++++++++------------------ WST-FC/src/DroneFactory.cpp | 48 +++++++++++++++++++++++++++++++++++ WST-FC/src/main.cpp | 46 +++++---------------------------- 4 files changed, 85 insertions(+), 65 deletions(-) create mode 100644 WST-FC/include/DroneFactory.h create mode 100644 WST-FC/src/DroneFactory.cpp diff --git a/WST-FC/include/DroneFactory.h b/WST-FC/include/DroneFactory.h new file mode 100644 index 0000000..57937e8 --- /dev/null +++ b/WST-FC/include/DroneFactory.h @@ -0,0 +1,16 @@ +#ifndef DRONEFACTORY_H +#define DRONEFACTORY_H + +#include +#include +#include "SensorsModule.h" +#include "modules/IModule.h" +#include "IMixer.h" + +class DroneFactory +{ +public: + static IMixer* BuildVehicle(SensorsModule& sensorsModule, std::vector& modules); +}; + +#endif \ No newline at end of file diff --git a/WST-FC/platformio.ini b/WST-FC/platformio.ini index 29ea818..e841f7c 100644 --- a/WST-FC/platformio.ini +++ b/WST-FC/platformio.ini @@ -1,40 +1,30 @@ -; PlatformIO Project Configuration File -; -; Build options: build flags, source filter -; Upload options: custom upload port, speed and extra flags -; Library options: dependencies, extra library storages -; Advanced options: extra scripting -; -; Please visit documentation for the other options and examples -; https://docs.platformio.org/page/projectconf.html - [env] platform = espressif32 -platform_packages = - framework-arduinoespressif32@https://github.com/maxgerhardt/pio-framework-bluepad32/archive/refs/heads/main.zip +platform_packages = + framework-arduinoespressif32@https://github.com/maxgerhardt/pio-framework-bluepad32/archive/refs/heads/main.zip framework = arduino monitor_speed = 115200 lib_deps = - adafruit/Adafruit MPU6050@^2.2.6 - adafruit/DHT sensor library@^1.4.6 - madhephaestus/ESP32Servo@^3.0.9 - br3ttb/PID@^1.2.1 - adafruit/Adafruit ADXL345@^1.3.4 - adafruit/Adafruit Unified Sensor@^1.1.14 - ciniml/WireGuard-ESP32 @ ^0.1.5 + adafruit/Adafruit MPU6050@^2.2.6 + adafruit/DHT sensor library@^1.4.6 + madhephaestus/ESP32Servo@^3.0.9 + br3ttb/PID@^1.2.1 + adafruit/Adafruit ADXL345@^1.3.4 + adafruit/Adafruit Unified Sensor@^1.1.14 + ciniml/WireGuard-ESP32 @ ^0.1.5 [env:esp32doit-devkit-v1] board = esp32doit-devkit-v1 -build_flags = - -D CORE_DEBUG_LEVEL=3 board_build.partitions = huge_app.csv [env:esp32-c3-super-mini] board = esp32-c3-devkitm-1 build_flags = - -D ARDUINO_USB_MODE=1 - -D ARDUINO_USB_CDC_ON_BOOT=1 - -D CORE_DEBUG_LEVEL=0 + -D ARDUINO_USB_MODE=1 + -D ARDUINO_USB_CDC_ON_BOOT=1 [env:esp32cam] -board = esp32cam \ No newline at end of file +board = esp32cam + +[platformio] +description = Advanced, gloriously over-engineered, and fundamentally unreasonable take on making things move. It is an open-source framework built to prove that the sweet spot of engineering lies exactly where "genuinely useful" meets "utterly insane." diff --git a/WST-FC/src/DroneFactory.cpp b/WST-FC/src/DroneFactory.cpp new file mode 100644 index 0000000..215fbed --- /dev/null +++ b/WST-FC/src/DroneFactory.cpp @@ -0,0 +1,48 @@ +#include "DroneFactory.h" + +#ifdef VEHICLE_TYPE_BICOPTER + #include "BicopterMixer.h" + #include "sensors/MpuSensor.h" + MpuSensor mpuSensor; +#endif + +#ifdef VEHICLE_TYPE_AIRBOAT + #include "AirBoatMixer.h" + #include "sensors/AdxlSensor.h" + #include "modules/CameraModule.h" + DCMotor motorL(16, 17, 4, 0); + DCMotor motorR(18, 19, 5, 1); + AdxlSensor adxlSensor; +#endif + +#ifdef VEHICLE_TYPE_TANK + #include "AirBoatMixer.h" + #include "modules/CameraModule.h" + DCMotor motorL(13, 14, 0); + DCMotor motorR(15, 2, 1); +#endif + +IMixer* DroneFactory::BuildVehicle(SensorsModule& sensorsModule, std::vector& modules) +{ + IMixer* droneMixer = nullptr; + + #ifdef VEHICLE_TYPE_BICOPTER + Serial.println("Configuring as BICOPTER"); + sensorsModule.AddSensor(&mpuSensor); + droneMixer = new BicopterMixer(); + #endif + + #ifdef VEHICLE_TYPE_AIRBOAT + Serial.println("Configuring as AIRBOAT"); + sensorsModule.AddSensor(&adxlSensor); + droneMixer = new BoatMixer(&motorL, &motorR); + modules.push_back(new CameraModule()); + #endif + + #ifdef VEHICLE_TYPE_TANK + Serial.println("Configuring as TANK"); + droneMixer = new BoatMixer(&motorL, &motorR); + #endif + + return droneMixer; +} \ No newline at end of file diff --git a/WST-FC/src/main.cpp b/WST-FC/src/main.cpp index 18f992b..8afeab1 100644 --- a/WST-FC/src/main.cpp +++ b/WST-FC/src/main.cpp @@ -5,26 +5,8 @@ #include "SensorsModule.h" #include "SensorsData.h" #include "modules/IModule.h" - -#ifdef VEHICLE_TYPE_BICOPTER - #include "BicopterMixer.h" - #include "sensors/MpuSensor.h" - MpuSensor mpuSensor; -#endif - -#ifdef VEHICLE_TYPE_AIRBOAT - #include "AirBoatMixer.h" - #include "sensors/AdxlSensor.h" - DCMotor motorL(16, 17, 4, 0); - DCMotor motorR(18, 19, 5, 1); -#endif - -#ifdef VEHICLE_TYPE_TANK - #include "AirBoatMixer.h" - #include "modules/CameraModule.h" - DCMotor motorL(13, 14, 0); - DCMotor motorR(15, 2, 1); -#endif +#include "IMixer.h" +#include "DroneFactory.h" DroneControlData droneControllData{}; SensorsData sensorsData{}; @@ -44,29 +26,13 @@ void setup() { Serial.begin(115200); - #ifdef VEHICLE_TYPE_BICOPTER - Serial.println("Configuring as BICOPTER"); - sensorsModule.AddSensor(&mpuSensor); - droneMixer = new BicopterMixer(); - #endif - #ifdef VEHICLE_TYPE_AIRBOAT - Serial.println("Configuring as AIRBOAT"); - sensorsModule.AddSensor(&adxlSensor); - droneMixer = new BoatMixer(&motorL, &motorR); - modules.push_back(new CameraModule()); - #endif - #ifdef VEHICLE_TYPE_TANK - Serial.println("Configuring as TANK"); - droneMixer = new BoatMixer(&motorL, &motorR); - #endif - comms.Init(); sensorsModule.Init(); - if(droneMixer != nullptr) droneMixer->Init(); - for (auto module : modules) - { + droneMixer = DroneFactory::BuildVehicle(sensorsModule, modules); + if(droneMixer != nullptr) + droneMixer->Init(); + for (auto module : modules) module->Init(); - } } void loop() From bea35bf8dcd2021c430624a0b55f7ccd9229dea1 Mon Sep 17 00:00:00 2001 From: Milosz Klim Date: Mon, 13 Apr 2026 22:47:44 +0200 Subject: [PATCH 2/4] More tweaks --- WST-FC/include/CommunicationModule.h | 4 +++- WST-FC/include/Configuration.h | 8 ++++---- .../communicationModules/CommunicationGamepadModule.h | 3 ++- WST-FC/include/modules/CameraModule.h | 2 +- WST-FC/platformio.ini | 8 ++++++++ WST-FC/src/CommunicationModule.cpp | 8 +++++--- WST-FC/src/DroneFactory.cpp | 8 +++++--- .../communicationModules/CommunicationGamepadModule.cpp | 4 +++- .../communicationModules/CommunicationWiFiUDPModule.cpp | 1 + WST-FC/src/modules/CameraModule.cpp | 8 ++++---- 10 files changed, 36 insertions(+), 18 deletions(-) diff --git a/WST-FC/include/CommunicationModule.h b/WST-FC/include/CommunicationModule.h index 94a5f33..2c514d4 100644 --- a/WST-FC/include/CommunicationModule.h +++ b/WST-FC/include/CommunicationModule.h @@ -9,7 +9,9 @@ #include "communicationModules\CommunicationWiFiUDPModule.h" #include "communicationModules\CommunicationESPNowModule.h" #include "communicationModules\CommunicationSerialModule.h" -#include "communicationModules\CommunicationGamepadModule.h" +#if (COMMUNICATION_METHOD == 3) + #include "communicationModules\CommunicationGamepadModule.h" +#endif class CommunicationModule { diff --git a/WST-FC/include/Configuration.h b/WST-FC/include/Configuration.h index 34cbd71..88f12ff 100644 --- a/WST-FC/include/Configuration.h +++ b/WST-FC/include/Configuration.h @@ -1,14 +1,14 @@ #ifndef CONFIGURATION_H #define CONFIGURATION_H -#define VEHICLE_TYPE_AIRBOAT +#define VEHICLE_TYPE_TANK -#define WIFI_SSID "AutisticDrones" -#define WIFI_PASSWORD "Autyzm2137" +#define WIFI_SSID "HPLOVER" +#define WIFI_PASSWORD "Autyzm2137!" #define UDP_CONTROLL_PORT 4210 #define MAX_ROGUE_TIME 1000 #define MIN_RSSI -80 -#define COMMUNICATION_METHOD 3 +#define COMMUNICATION_METHOD 0 #define TELEMETRY_TIME 1000 enum DroneStatus diff --git a/WST-FC/include/communicationModules/CommunicationGamepadModule.h b/WST-FC/include/communicationModules/CommunicationGamepadModule.h index 46a028e..c432aa7 100644 --- a/WST-FC/include/communicationModules/CommunicationGamepadModule.h +++ b/WST-FC/include/communicationModules/CommunicationGamepadModule.h @@ -1,6 +1,6 @@ #ifndef COMMUNICATIONGAMEPADMODULE_H #define COMMUNICATIONGAMEPADMODULE_H - +#ifndef NOT_USE_BLUESTACK #include "DroneData.h" #include "ICommunicationInterface.h" #include "Configuration.h" @@ -27,4 +27,5 @@ class CommunicationGamepadModule : public ICommunicationInterface { static void onConnectedGamepad(GamepadPtr gp); static void onDisconnectedGamepad(GamepadPtr gp); }; +#endif #endif \ No newline at end of file diff --git a/WST-FC/include/modules/CameraModule.h b/WST-FC/include/modules/CameraModule.h index d1fa098..906f406 100644 --- a/WST-FC/include/modules/CameraModule.h +++ b/WST-FC/include/modules/CameraModule.h @@ -27,7 +27,7 @@ #define HREF_GPIO_NUM 23 #define PCLK_GPIO_NUM 22 #define XCLK_FREQ_HZ 20000000 - #define JPEG_QUALITY 25 + #define JPEG_QUALITY 45 #define FB_COUNT 2 #define PART_BOUNDARY "123456789000000000000987654321" diff --git a/WST-FC/platformio.ini b/WST-FC/platformio.ini index e841f7c..97ed27e 100644 --- a/WST-FC/platformio.ini +++ b/WST-FC/platformio.ini @@ -23,8 +23,16 @@ build_flags = -D ARDUINO_USB_MODE=1 -D ARDUINO_USB_CDC_ON_BOOT=1 +#We have much problems with bluestack on espcam with cam enabled [env:esp32cam] board = esp32cam +platform_packages = + framework-arduinoespressif32 +framework = arduino +build_flags = + -D NOT_USE_BLUESTACK + -D BOARD_HAS_PSRAM + -mfix-esp32-psram-cache-issue [platformio] description = Advanced, gloriously over-engineered, and fundamentally unreasonable take on making things move. It is an open-source framework built to prove that the sweet spot of engineering lies exactly where "genuinely useful" meets "utterly insane." diff --git a/WST-FC/src/CommunicationModule.cpp b/WST-FC/src/CommunicationModule.cpp index b2adbfc..a48eb74 100644 --- a/WST-FC/src/CommunicationModule.cpp +++ b/WST-FC/src/CommunicationModule.cpp @@ -18,9 +18,11 @@ void CommunicationModule::Init() if(COMMUNICATION_METHOD == 2){ communicationInterface = new CommunicationSerialModule(sharedData, droneStatus); } - if(COMMUNICATION_METHOD == 3){ - communicationInterface = new CommunicationGamepadModule(sharedData, droneStatus); - } + #if (COMMUNICATION_METHOD == 3) + if(COMMUNICATION_METHOD == 3){ + communicationInterface = new CommunicationGamepadModule(sharedData, droneStatus); + } + #endif if(communicationInterface == nullptr) { diff --git a/WST-FC/src/DroneFactory.cpp b/WST-FC/src/DroneFactory.cpp index 215fbed..0c63a1d 100644 --- a/WST-FC/src/DroneFactory.cpp +++ b/WST-FC/src/DroneFactory.cpp @@ -9,7 +9,6 @@ #ifdef VEHICLE_TYPE_AIRBOAT #include "AirBoatMixer.h" #include "sensors/AdxlSensor.h" - #include "modules/CameraModule.h" DCMotor motorL(16, 17, 4, 0); DCMotor motorR(18, 19, 5, 1); AdxlSensor adxlSensor; @@ -18,8 +17,9 @@ #ifdef VEHICLE_TYPE_TANK #include "AirBoatMixer.h" #include "modules/CameraModule.h" + #include "sensors/HCSR04Sensor.h" DCMotor motorL(13, 14, 0); - DCMotor motorR(15, 2, 1); + DCMotor motorR(15, 2, 1); #endif IMixer* DroneFactory::BuildVehicle(SensorsModule& sensorsModule, std::vector& modules) @@ -36,12 +36,14 @@ IMixer* DroneFactory::BuildVehicle(SensorsModule& sensorsModule, std::vector Date: Fri, 17 Apr 2026 15:35:39 +0200 Subject: [PATCH 3/4] Added custom python script to configure bluestack32 --- WST-FC/configuration_preprocesor.py | 8 ++++++++ .../CommunicationGamepadModule.h | 2 +- WST-FC/platformio.ini | 12 +++++------- WST-FC/src/DroneFactory.cpp | 2 +- .../CommunicationGamepadModule.cpp | 2 +- 5 files changed, 16 insertions(+), 10 deletions(-) create mode 100644 WST-FC/configuration_preprocesor.py diff --git a/WST-FC/configuration_preprocesor.py b/WST-FC/configuration_preprocesor.py new file mode 100644 index 0000000..b767aa3 --- /dev/null +++ b/WST-FC/configuration_preprocesor.py @@ -0,0 +1,8 @@ +Import("env") +use_bt = env.GetProjectOption("use_bluestack", "no") + +if use_bt == "yes": + env.Append(CPPDEFINES=["USE_BLUEPAD32"]) + env.Append(PIOPLATFORMPACKAGES=[ + "framework-arduinoespressif32@https://github.com/maxgerhardt/pio-framework-bluepad32/archive/refs/heads/main.zip" + ]) \ No newline at end of file diff --git a/WST-FC/include/communicationModules/CommunicationGamepadModule.h b/WST-FC/include/communicationModules/CommunicationGamepadModule.h index c432aa7..1327c8e 100644 --- a/WST-FC/include/communicationModules/CommunicationGamepadModule.h +++ b/WST-FC/include/communicationModules/CommunicationGamepadModule.h @@ -1,6 +1,6 @@ #ifndef COMMUNICATIONGAMEPADMODULE_H #define COMMUNICATIONGAMEPADMODULE_H -#ifndef NOT_USE_BLUESTACK +#ifdef USE_BLUEPAD32 #include "DroneData.h" #include "ICommunicationInterface.h" #include "Configuration.h" diff --git a/WST-FC/platformio.ini b/WST-FC/platformio.ini index 97ed27e..753e370 100644 --- a/WST-FC/platformio.ini +++ b/WST-FC/platformio.ini @@ -1,9 +1,12 @@ +[platformio] +description = Advanced, gloriously over-engineered, and fundamentally unreasonable take on making things move. It is an open-source framework built to prove that the sweet spot of engineering lies exactly where "genuinely useful" meets "utterly insane." + [env] platform = espressif32 -platform_packages = - framework-arduinoespressif32@https://github.com/maxgerhardt/pio-framework-bluepad32/archive/refs/heads/main.zip framework = arduino monitor_speed = 115200 +extra_scripts = pre:configuration_preprocesor.py +use_bluestack = no lib_deps = adafruit/Adafruit MPU6050@^2.2.6 adafruit/DHT sensor library@^1.4.6 @@ -23,16 +26,11 @@ build_flags = -D ARDUINO_USB_MODE=1 -D ARDUINO_USB_CDC_ON_BOOT=1 -#We have much problems with bluestack on espcam with cam enabled [env:esp32cam] board = esp32cam platform_packages = framework-arduinoespressif32 framework = arduino build_flags = - -D NOT_USE_BLUESTACK -D BOARD_HAS_PSRAM -mfix-esp32-psram-cache-issue - -[platformio] -description = Advanced, gloriously over-engineered, and fundamentally unreasonable take on making things move. It is an open-source framework built to prove that the sweet spot of engineering lies exactly where "genuinely useful" meets "utterly insane." diff --git a/WST-FC/src/DroneFactory.cpp b/WST-FC/src/DroneFactory.cpp index 0c63a1d..0617943 100644 --- a/WST-FC/src/DroneFactory.cpp +++ b/WST-FC/src/DroneFactory.cpp @@ -43,7 +43,7 @@ IMixer* DroneFactory::BuildVehicle(SensorsModule& sensorsModule, std::vector Date: Mon, 4 May 2026 18:49:24 +0200 Subject: [PATCH 4/4] Added system registry --- WST-FC/include/CommunicationModule.h | 2 +- WST-FC/include/Configuration.h | 15 ++- WST-FC/include/SystemRegistry.h | 92 +++++++++++++++++++ .../CommunicationESPNowModule.h | 3 - .../CommunicationGamepadModule.h | 2 - .../CommunicationSerialModule.h | 7 +- .../CommunicationWiFiUDPModule.h | 3 - .../ICommunicationInterface.h | 39 ++++++++ WST-FC/platformio.ini | 3 + WST-FC/src/CommunicationModule.cpp | 46 +++++----- WST-FC/src/DroneFactory.cpp | 6 +- .../CommunicationESPNowModule.cpp | 3 +- .../CommunicationSerialModule.cpp | 18 ++-- .../CommunicationWiFiUDPModule.cpp | 9 +- WST-FC/src/main.cpp | 13 ++- 15 files changed, 198 insertions(+), 63 deletions(-) create mode 100644 WST-FC/include/SystemRegistry.h diff --git a/WST-FC/include/CommunicationModule.h b/WST-FC/include/CommunicationModule.h index 2c514d4..e959b15 100644 --- a/WST-FC/include/CommunicationModule.h +++ b/WST-FC/include/CommunicationModule.h @@ -17,7 +17,7 @@ class CommunicationModule { private: DroneControlData *sharedData; - ICommunicationInterface *communicationInterface; + std::vector communicationInterfaces; wl_status_t connectionStatus{WL_IDLE_STATUS}; DroneStatus *droneStatus; diff --git a/WST-FC/include/Configuration.h b/WST-FC/include/Configuration.h index 88f12ff..e249e8f 100644 --- a/WST-FC/include/Configuration.h +++ b/WST-FC/include/Configuration.h @@ -3,14 +3,15 @@ #define VEHICLE_TYPE_TANK -#define WIFI_SSID "HPLOVER" -#define WIFI_PASSWORD "Autyzm2137!" -#define UDP_CONTROLL_PORT 4210 #define MAX_ROGUE_TIME 1000 #define MIN_RSSI -80 -#define COMMUNICATION_METHOD 0 #define TELEMETRY_TIME 1000 +#define USE_WIFI_UDP +//#define USE_ESP_NOW +#define USE_SERIAL +//#define USE_GAMEPAD + enum DroneStatus { WORKS, @@ -28,4 +29,10 @@ enum DroneStatus #define WG_PEER_PUBLIC_KEY "KLUCZ_PUBLICZNY_SERWERA=" #endif +#if defined(USE_WIFI_UDP) || defined(USE_ESP_NOW) + #define WIFI_SSID "HPLOVER" + #define WIFI_PASSWORD "Autyzm2137!" + #define UDP_CONTROLL_PORT 4210 +#endif + #endif \ No newline at end of file diff --git a/WST-FC/include/SystemRegistry.h b/WST-FC/include/SystemRegistry.h new file mode 100644 index 0000000..4f4e271 --- /dev/null +++ b/WST-FC/include/SystemRegistry.h @@ -0,0 +1,92 @@ +#ifndef SYSTEM_REGISTRY_H +#define SYSTEM_REGISTRY_H + +#include +#include + +enum ParamType { + PARAM_INT, + PARAM_FLOAT, + PARAM_DOUBLE, + PARAM_BOOL +}; + +struct ConfigParam { + const char* name; + void* ptr; + ParamType type; +}; + +class SystemRegistry { +private: + std::vector params; + SystemRegistry() {} + +public: + static SystemRegistry& GetInstance() { + static SystemRegistry instance; + return instance; + } + + void Register(const char* name, void* ptr, ParamType type) + { + params.push_back({name, ptr, type}); + } + + bool SetValue(const char* name, const char* valueStr) + { + for(const auto& param : params) + { + if(strcmp(param.name, name) == 0) + { + switch (param.type) { + case PARAM_INT: + *(int*)param.ptr = atoi(valueStr); + break; + case PARAM_FLOAT: + *(float*)param.ptr = atof(valueStr); + break; + case PARAM_DOUBLE: + *(double*)param.ptr = atof(valueStr); + break; + case PARAM_BOOL: + *(bool*)param.ptr = (atoi(valueStr) > 0); + break; + } + Serial.println("Value SET"); + return true; + } + } + Serial.println("There is no such variable"); + return false; + } + + void ReturnAllRegistryNames() + { + for(const auto& param : params) + { + Serial.print(param.name); + Serial.print(": "); + + switch (param.type) { + case PARAM_INT: + Serial.println(*(int*)param.ptr); + break; + case PARAM_FLOAT: + Serial.println(*(float*)param.ptr); + break; + case PARAM_DOUBLE: + Serial.println(*(double*)param.ptr); + break; + case PARAM_BOOL: + Serial.println(*(bool*)param.ptr); + break; + default: + Serial.println("UNSUPPORTED_TYPE"); + break; + } + } + } +}; + +#endif \ No newline at end of file diff --git a/WST-FC/include/communicationModules/CommunicationESPNowModule.h b/WST-FC/include/communicationModules/CommunicationESPNowModule.h index db6f052..37b1d50 100644 --- a/WST-FC/include/communicationModules/CommunicationESPNowModule.h +++ b/WST-FC/include/communicationModules/CommunicationESPNowModule.h @@ -11,9 +11,6 @@ class CommunicationESPNowModule : public ICommunicationInterface{ private: static CommunicationESPNowModule* instance; - DroneControlData *sharedData; - unsigned long lastUpdate{0}; - DroneStatus *droneStatus; ulong lastDataTime = 0; uint8_t broadcastAddress[6] {0xEC,0x64,0xC9,0xC4,0xA2,0x1A}; public: diff --git a/WST-FC/include/communicationModules/CommunicationGamepadModule.h b/WST-FC/include/communicationModules/CommunicationGamepadModule.h index 1327c8e..e3942d8 100644 --- a/WST-FC/include/communicationModules/CommunicationGamepadModule.h +++ b/WST-FC/include/communicationModules/CommunicationGamepadModule.h @@ -9,8 +9,6 @@ class CommunicationGamepadModule : public ICommunicationInterface { private: - DroneControlData *sharedData; - DroneStatus *droneStatus; GamepadPtr myGamepad = nullptr; public: diff --git a/WST-FC/include/communicationModules/CommunicationSerialModule.h b/WST-FC/include/communicationModules/CommunicationSerialModule.h index cf76732..6c5a5d0 100644 --- a/WST-FC/include/communicationModules/CommunicationSerialModule.h +++ b/WST-FC/include/communicationModules/CommunicationSerialModule.h @@ -8,14 +8,9 @@ class CommunicationSerialModule : public ICommunicationInterface{ private: - DroneControlData *sharedData; - unsigned long lastUpdate{0}; - DroneStatus *droneStatus; long lastDataTime = 0; - uint8_t inputBuffer[sizeof(DroneControlData)]; + uint8_t inputBuffer[128]; - const uint8_t HEADER_BYTE_1 = 0x44; // 'D' - const uint8_t HEADER_BYTE_2 = 0x43; // 'C' public: CommunicationSerialModule(DroneControlData *dataPtr, DroneStatus *status); void Init() override; diff --git a/WST-FC/include/communicationModules/CommunicationWiFiUDPModule.h b/WST-FC/include/communicationModules/CommunicationWiFiUDPModule.h index a376da8..b492b61 100644 --- a/WST-FC/include/communicationModules/CommunicationWiFiUDPModule.h +++ b/WST-FC/include/communicationModules/CommunicationWiFiUDPModule.h @@ -11,15 +11,12 @@ #endif class CommunicationWiFiUDPModule : public ICommunicationInterface{ private: - DroneControlData *sharedData; WiFiUDP udp; unsigned int localPort{0}; char packetBuffer[255]; - unsigned long lastUpdate{0}; int8_t rssi{0}; wl_status_t connectionStatus{WL_IDLE_STATUS}; - DroneStatus *droneStatus; IPAddress remoteIP; unsigned int remotePort {0}; diff --git a/WST-FC/include/communicationModules/ICommunicationInterface.h b/WST-FC/include/communicationModules/ICommunicationInterface.h index 62a82ee..3d38e58 100644 --- a/WST-FC/include/communicationModules/ICommunicationInterface.h +++ b/WST-FC/include/communicationModules/ICommunicationInterface.h @@ -2,7 +2,46 @@ #define ICOMMUNICATIONINTERFACE #include "../SensorsData.h" +#include "SystemRegistry.h" +#include "Configuration.h" class ICommunicationInterface{ +protected: + DroneControlData *sharedData; + DroneStatus *droneStatus; + unsigned long lastUpdate{0}; + + void ParseIncomingBytes(uint8_t* buffer, size_t len) + { + if (sharedData == nullptr || len < 2) return; + + if (len == sizeof(DroneControlData)) + { + memcpy(sharedData, buffer, sizeof(DroneControlData)); + lastUpdate = millis(); + } + else if (buffer[0] == 'S' && buffer[1] == 'E' && buffer[2] == 'T') + { + if (len < 64) buffer[len] = '\0'; + else buffer[63] = '\0'; + + char* str = (char*)buffer; + char* cmd = strtok(str, " "); + char* paramName = strtok(NULL, " "); + char* valueStr = strtok(NULL, " "); + + if (paramName != nullptr && valueStr != nullptr) + { + SystemRegistry::GetInstance().SetValue(paramName, valueStr); + } + } + else if (buffer[0] == 'G' && buffer[1] == 'E' && buffer[2] == 'T') + { + if (len < 64) buffer[len] = '\0'; + else buffer[63] = '\0'; + SystemRegistry::GetInstance().ReturnAllRegistryNames(); + } + } + public: virtual ~ICommunicationInterface(){} virtual void Init(); diff --git a/WST-FC/platformio.ini b/WST-FC/platformio.ini index 753e370..d5a6f26 100644 --- a/WST-FC/platformio.ini +++ b/WST-FC/platformio.ini @@ -7,6 +7,9 @@ framework = arduino monitor_speed = 115200 extra_scripts = pre:configuration_preprocesor.py use_bluestack = no +monitor_echo = yes +monitor_filters = send_on_enter +build_flags = -DCORE_DEBUG_LEVEL=5 lib_deps = adafruit/Adafruit MPU6050@^2.2.6 adafruit/DHT sensor library@^1.4.6 diff --git a/WST-FC/src/CommunicationModule.cpp b/WST-FC/src/CommunicationModule.cpp index a48eb74..2ee1666 100644 --- a/WST-FC/src/CommunicationModule.cpp +++ b/WST-FC/src/CommunicationModule.cpp @@ -9,36 +9,40 @@ CommunicationModule::CommunicationModule(DroneControlData *dataPtr, DroneStatus void CommunicationModule::Init() { - if(COMMUNICATION_METHOD == 0){ - communicationInterface = new CommunicationWiFiUDPModule(sharedData, UDP_CONTROLL_PORT, droneStatus); - } - if(COMMUNICATION_METHOD == 1){ - communicationInterface = new CommunicationESPNowModule(sharedData, droneStatus); - } - if(COMMUNICATION_METHOD == 2){ - communicationInterface = new CommunicationSerialModule(sharedData, droneStatus); - } - #if (COMMUNICATION_METHOD == 3) - if(COMMUNICATION_METHOD == 3){ - communicationInterface = new CommunicationGamepadModule(sharedData, droneStatus); - } + #ifdef USE_WIFI_UDP + communicationInterfaces.push_back(new CommunicationWiFiUDPModule(sharedData, UDP_CONTROLL_PORT, droneStatus)); + #endif + + #ifdef USE_ESP_NOW + communicationInterfaces.push_back( new CommunicationESPNowModule(sharedData, droneStatus)); + #endif + + #ifdef USE_SERIAL + communicationInterfaces.push_back(new CommunicationSerialModule(sharedData, droneStatus)); #endif - if(communicationInterface == nullptr) + #ifdef USE_GAMEPAD + communicationInterfaces.push_back(new CommunicationGamepadModule(sharedData, droneStatus)); + #endif + + for(auto interface : communicationInterfaces) { - //Critical error!!! - }else{ - communicationInterface->Init(); + interface->Init(); } + } void CommunicationModule::Loop() { - if(communicationInterface == nullptr) return; - communicationInterface -> Loop(); + for(auto interface : communicationInterfaces) + { + interface->Loop(); + }; } void CommunicationModule::SendData(SensorsData* data) { - if(communicationInterface == nullptr) return; - communicationInterface->SendData(data); + for(auto interface : communicationInterfaces) + { + interface->SendData(data); + }; } \ No newline at end of file diff --git a/WST-FC/src/DroneFactory.cpp b/WST-FC/src/DroneFactory.cpp index 0617943..3c0f38e 100644 --- a/WST-FC/src/DroneFactory.cpp +++ b/WST-FC/src/DroneFactory.cpp @@ -40,10 +40,10 @@ IMixer* DroneFactory::BuildVehicle(SensorsModule& sensorsModule, std::vector 0) + { + size_t bytesToRead = min(bytesAvailable, (int)sizeof(inputBuffer)); + size_t bytesRead = Serial.readBytes(inputBuffer, bytesToRead); + ParseIncomingBytes((uint8_t*)inputBuffer, bytesRead); + } if ((millis() - lastUpdate) > MAX_ROGUE_TIME) { diff --git a/WST-FC/src/communicationModules/CommunicationWiFiUDPModule.cpp b/WST-FC/src/communicationModules/CommunicationWiFiUDPModule.cpp index d57b021..15ccd3b 100644 --- a/WST-FC/src/communicationModules/CommunicationWiFiUDPModule.cpp +++ b/WST-FC/src/communicationModules/CommunicationWiFiUDPModule.cpp @@ -58,15 +58,8 @@ void CommunicationWiFiUDPModule::Loop() if (packetSize) { - int len = udp.read(packetBuffer, 255); + ParseIncomingBytes((uint8_t*)packetBuffer, packetSize); - if (len != sizeof(DroneControlData)) - { - Serial.printf("Error: invalid packet"); - return; - } - memcpy(sharedData, packetBuffer, sizeof(DroneControlData)); - lastUpdate = millis(); remoteIP = udp.remoteIP(); remotePort = udp.remotePort(); } diff --git a/WST-FC/src/main.cpp b/WST-FC/src/main.cpp index 8afeab1..e258cba 100644 --- a/WST-FC/src/main.cpp +++ b/WST-FC/src/main.cpp @@ -25,14 +25,25 @@ unsigned long lastTelemetryTimestamp {0}; void setup() { Serial.begin(115200); - + Serial.println("Starting"); + Serial.flush(); comms.Init(); + Serial.println("Comms initiated"); + Serial.flush(); sensorsModule.Init(); + Serial.println("Sensors initiated"); + Serial.flush(); droneMixer = DroneFactory::BuildVehicle(sensorsModule, modules); if(droneMixer != nullptr) droneMixer->Init(); + Serial.println("Mixer initiated"); + Serial.flush(); for (auto module : modules) module->Init(); + Serial.println("Modules initiated"); + Serial.flush(); + Serial.println("Ok"); + Serial.flush(); } void loop()