fix reconnect gamepad

.vscode/settings.json

@@ -83,6 +83,7 @@
         "Ahnung",
         "akku",
         "blox",
+        "bluedroid",
         "gast",
         "GNSS",
         "GPGGA",

doc/Notes and TODOs/TODO allgemein.txt

@@ -3,4 +3,6 @@
     -> Program battery pack tracking
     -> Add an beeper
     -> Program the beeper
+    -> GPS-Menu without Wlan NtripClient
+    -> CaptureRoute Modus crash when someone try's to save a point
 

doc/Notes and TODOs/ps3 reconnect.txt

@@ -0,0 +1,26 @@
+ps3.h
+void ps3ResetGlobals();
+
+ps3.c
+void ps3ResetGlobals() {
+    // Own Code
+    is_active = false;
+    ps3_connection_cb = NULL;
+    ps3_connection_object_cb = NULL;
+    ps3_connection_object = NULL;
+    ps3_event_cb = NULL;
+    ps3_event_object_cb = NULL;
+    ps3_event_object = NULL;
+}
+
+Ps3Controller.h
+private
+void resetGlobals();
+
+Ps3Controller.cpp
+void Ps3Controller::resetGlobals() {
+    ps3ResetGlobals();
+}
+
+in -> Ps3Controller::begin
+this->resetGlobals();

include/config.h

@@ -15,6 +15,14 @@
  * The PS3-Controller only connects to the address which is saved in it.
  */
 #define CONTROLLER_MAC "24:62:AB:F2:4B:3A"
+
+/**
+ * @brief Prevents creation of the global object
+ * 
+ * The PS3-Controller class creates a global object to use.
+ */
+#define NO_GLOBAL_INSTANCES
+
 #define UBLOX_GNSS_SPI_CS 17
 #define UBLOX_GNSS_SPI_COPI 16
 #define UBLOX_GNSS_SPI_CIPO 4

lib/Navigation/navigation.cpp

@@ -80,13 +80,13 @@ void Navigation::loop() {
         this->updateCurrentLocation();
         std::cout << "duration between: " << millis() - lastTime << std::endl;
         lastTime = millis();
-    } else if (millis() - lastTime > 20000) {
+    } else if (millis() - lastTime > 20000 && !this->gps->getGnssFixOk()) {
-        std::cout << "Keine Daten mehr!" << std::endl;
+        std::cout << "No Data from ZED arrived since a long time. Navigation::loop" << std::endl;
         this->gps->flushPVT();
         this->gps->softwareResetGNSSOnly();
         lastTime = millis();
 
-        // std::cout
+        //std::cout
         //     << "checkUblox: " << this->gps->checkUblox() << "  -|-  "
         //     << "getGnssFixOk: " << this->gps->getGnssFixOk() << "  -|-  "
         //     << "getHWstatus: " << this->gps->getHWstatus() << "  -|-  "

lib/Times/debugTimes.cpp

@@ -16,7 +16,7 @@ bool DebugTimes::printWarning = true;
 DebugTimes::DebugTimes() {
     this->startTime = millis();
 
-    if (!DebugTimes::printWarning) {
+    if (DebugTimes::printWarning) {
         std::cout << std::endl << "Warning: DebugTimes is muuted, no times are be shown." << std::endl << std::endl;
         DebugTimes::printWarning = false;
     }

src/SpecialMenus/Akku/menuAkku.cpp

@@ -11,8 +11,9 @@
 
 #include "menuAkku.h"
 
-MenuAkku::MenuAkku(Battery* mainBattery) {
+MenuAkku::MenuAkku(Battery* mainBattery, Ps3Controller* gamepad) {
     this->mainBattery = mainBattery;
+    this->gamepad = gamepad;
 }
 
 void MenuAkku::right() {
@@ -38,8 +39,8 @@ void MenuAkku::printMenu() {
     uint8_t mainBattery = this->mainBattery->getBatteryPercent();
     double mainBatteryVolt = this->mainBattery->getBatteryVoltage();
 
-    if( controllerBattery != Ps3.data.status.battery ){
+    if( controllerBattery != this->gamepad->data.status.battery ){
-        controllerBattery = Ps3.data.status.battery;
+        controllerBattery = this->gamepad->data.status.battery;
     }
 
     if( controllerBattery == ps3_status_battery_charging )      sprintf(buf, "LOAD");

src/SpecialMenus/Akku/menuAkku.h

@@ -30,7 +30,7 @@ class MenuAkku : public MenuControl {
          * @brief Construct a new Menu Akku object
          * 
          */
-        MenuAkku(Battery* mainBattery);
+        MenuAkku(Battery* mainBattery, Ps3Controller* gamepad);
 
         void down() override {}
         void up() override {}
@@ -58,6 +58,7 @@ class MenuAkku : public MenuControl {
         uint32_t lastMillis = 0;
 
         Battery* mainBattery;
+        Ps3Controller* gamepad;
 
 };
 

src/driveModi/driveManager.cpp

@@ -39,7 +39,7 @@ void DriveManager::loop() {
 
     DebugTimes navigationTime;
     this->navigation->loop();
-    navigationTime.stopConsol("NavigationTime", 5);
+    navigationTime.stopConsol("NavigationTime", 20);
 
     if (currentModusPtr)
         this->currentModusPtr->loop();

src/main.cpp

@@ -14,14 +14,16 @@
  */
 
 #include <Arduino.h>
+
+#include "config.h"
+
 #include <Ps3Controller.h>
 #include <iostream>
 #include <Wire.h>
 #include <LiquidCrystal_I2C.h>
-#include <BluetoothSerial.h>
 #include <ESP32Ping.h>
-
+#include <BluetoothSerial.h>
-#include "config.h"
+#include <esp_bt_main.h>
 
 #include "driveModi/driveManager.h"
 #include "OutputBuf/outputBuf.h"
@@ -47,11 +49,15 @@ Menu* main_m;
 LiquidCrystal_I2C* lcd;
 OutputBuf* outputBuf;
 DebugMqtt* debugMqtt = nullptr;
-BluetoothSerial* serialBT = nullptr;
 Battery* mainBattery;
+Ps3Controller* gamepad;
+BluetoothSerial SerialBT;
 
 bool wifiIsActive;
+bool disconnectPs3 = false;
 
+
+void configureController(void);
 void callbackControllerAction(void);
 void callbackControllerConnect(void);
 void callbackControllerDisconnect(void);
@@ -61,7 +67,7 @@ void makeMenu(void);
 
 void setup() {
     Serial.begin(115200);
-    // DebugTimes::setConsolOutput(true);
+    DebugTimes::setConsolOutput(true);
     DebugTimes setupTime;
     char wifiIndicator = 'X';
 
@@ -104,18 +110,9 @@ void setup() {
     std::cout << "Welcome to Kleiax-Rover" << std::endl;
     std::cout << "All actions from the main program run on Core -> " << xPortGetCoreID() << std::endl;
 
-    Ps3.attach(callbackControllerAction);
+    gamepad = new Ps3Controller;
-    Ps3.attachOnConnect(callbackControllerConnect);
+    configureController();
-    Ps3.attachOnDisconnect(callbackControllerDisconnect);
-    std::cout << "\nReady to connect a PS3 Controller... \n";
 
-    // Need to be connected befor an other device connect
-    Ps3.begin(CONTROLLER_MAC);
-
-    std::cout << "Activate additional output via Bluetooth..." << std::endl;
-    serialBT = new BluetoothSerial;
-    serialBT->begin("Kleiax-Rover");
-    outputBuf->setSerialBT(serialBT);
     outputBuf->activateMqtt(false);
 
     lcd->backlight();
@@ -141,11 +138,26 @@ void loop() {
     mainBattery->loop();
     mainBatteryTime.stopConsol("MainBatteryTime", 5);
 
-    // static uint32_t lastMillis = 0;
+    if (disconnectPs3) {
-    // if (millis() - lastMillis > 2000) {
+        DebugTimes disconnectPS3Time;
-    //     std::cout << "Is BT-Consol connected: " << serialBT->connected() << std::endl;
+        std::cout << "Disconnect PS3 Controller." << std::endl;
-    //     lastMillis = millis();
+        gamepad->end();
-    // }
+        esp_bluedroid_disable();
+        esp_bluedroid_deinit();
+        configureController();
+        disconnectPs3 = false;
+        disconnectPS3Time.stopConsol("Ps3 Disconnect");
+    }
+}
+
+void configureController() {
+    
+    gamepad->attach(callbackControllerAction);
+    gamepad->attachOnConnect(callbackControllerConnect);
+    gamepad->attachOnDisconnect(callbackControllerDisconnect);
+    std::cout << "\nReady to connect a PS3 Controller... \n";
+
+    gamepad->begin(CONTROLLER_MAC);
 }
 
 void callbackControllerAction() {
@@ -153,7 +165,7 @@ void callbackControllerAction() {
         static uint32_t lastMillis = 0;
         if (reset)
             lastMillis = millis();
-        static const uint16_t delay = 300;
+        static const uint16_t delay = 100;
         bool res = false;
 
         if (millis() - lastMillis > delay)
@@ -163,19 +175,19 @@ void callbackControllerAction() {
 
     // Menu controlling
     if (isDelayReached()) {
-        if (Ps3.data.button.up)
+        if (gamepad->data.button.up)
             main_m->up();
-        else if (Ps3.data.button.down)
+        else if (gamepad->data.button.down)
             main_m->down();
-        else if (Ps3.data.button.left)
+        else if (gamepad->data.button.left)
             main_m->left();
-        else if (Ps3.data.button.right)
+        else if (gamepad->data.button.right)
             main_m->right();
-        else if (Ps3.data.button.circle)
+        else if (gamepad->data.button.circle)
             main_m->yes();
-        else if (Ps3.data.button.cross)
+        else if (gamepad->data.button.cross)
             main_m->no();
-        else if (Ps3.data.button.select)
+        else if (gamepad->data.button.select)
             debugMqtt->sendMsg(Loglevel::info, "Here we are: callbackController..");
 
         isDelayReached(true);        
@@ -185,18 +197,23 @@ void callbackControllerAction() {
 }
 
 void callbackControllerConnect() {
+    static bool firstConnect = true;
+
     std::cout << "Controller connected to ESP32" << std::endl;
     std::cout << "All actions from the Controller run on Core -> " << xPortGetCoreID() << std::endl;
 
     // Display is not functional without this seconde init, first init is in setup(),
     // because after the setup the other core control the display.
+    if (firstConnect) {
         lcd->init();
         main_m->printMenu();
+        firstConnect = false;
+    }
 }
 
 void callbackControllerDisconnect() {
     std::cout << "Controller disconnected..." << std::endl;
-    // Ps3.end();
+    // gamepad->end();
 }
 
 void i2cScanner() {
@@ -243,7 +260,7 @@ void makeMenu() {
     };
 
     auto disconnectController = [&]() {
-        Ps3.end();
+        disconnectPs3 = true;
     };
 
     // Create Menu
@@ -257,7 +274,7 @@ void makeMenu() {
     MenuAutopilot* auto_m = new MenuAutopilot(driveManager);
     MenuTestMode* test_m = new MenuTestMode(driveManager);
     MenuGPS* gps_m = new MenuGPS(driveManager);
-    MenuAkku* akku_m = new MenuAkku(mainBattery);
+    MenuAkku* akku_m = new MenuAkku(mainBattery, gamepad);
 
     // Set Menus on LCD
     main_m->setLcd(lcd);