diff --git a/doc/TODO allgemein.txt b/doc/TODO allgemein.txt index ae1a25d..cfe6422 100644 --- a/doc/TODO allgemein.txt +++ b/doc/TODO allgemein.txt @@ -10,7 +10,9 @@ Do later: -> Engine slow down without curve in motorControl -> Network clean up (Mqtt remove?) -> Extra class for maneuver, autopilot should inherit from int16_t - -> Better remote Control with Leds for gnss rtk etc + -> Remote Control + - Leds for gnss rtk etc + - what happens exactly when no data is arriving -> Test Menu for big curve driving -> Api with Names and show on Maps in Browser -> Menu structure mit add functions for each menu mit pointer return to config (additional not replace) @@ -27,21 +29,18 @@ Do later: -> Menü für Einstellungen - PID - Geschwindigkeiten + - WiFi (save in Flash) -> Battery - tabelle mit eigenen Werten übergeben und nicht in header (wiederverwendbarkeit) - kalibrierungsmethode mit Menü - Daten in flash speichern können + -> Check speration between Ui and Route (RouteMenu) Do now: Code: Doxygen Kommentare aktualisieren - Automat in MoveControl weil jetzt in Arbeit beschrieben - Liste mit Betriebsmodi, automatisch in Menü einfügen - battery bruch mit R werten nur einmal berechnen - lange kein daten von fernbedienung dann? - Input pointer von fernbedienung nicht veränderbar doppel const - trennen funktion und ui route menü + Add Gyroskop Sensor Latex: Genaue Beschreibung von PulseCounter HardwareUnit wenn möglich diff --git a/lib/Battery/battery.cpp b/lib/Battery/battery.cpp index 5a7bd96..1038698 100644 --- a/lib/Battery/battery.cpp +++ b/lib/Battery/battery.cpp @@ -15,7 +15,7 @@ Battery::Battery(uint8_t pin, uint32_t r1, uint32_t r2) { this->pin = pin; this->r1 = r1; this->r2 = r2; - this->batteryVoltageFactor = (double) (this->r1 + this->r2)) / (double) this->r2 + this->batteryVoltageFactor = (double) (this->r1 + this->r2) / (double) this->r2; this->initBuffer(); this->loopDelay = 100; } @@ -72,7 +72,7 @@ double Battery::calculateInputVoltage() { void Battery::calculateBatteryVoltage() { if (this->r1 && this->r2) { - this->batteryVoltage = (this->calculateInputVoltage() * this->batteryVoltageFactor; + this->batteryVoltage = this->calculateInputVoltage() * this->batteryVoltageFactor; return; } diff --git a/lib/Sensors/sensorData.cpp b/lib/Sensors/sensorData.cpp index 5ca4809..22ddd58 100644 --- a/lib/Sensors/sensorData.cpp +++ b/lib/Sensors/sensorData.cpp @@ -70,7 +70,32 @@ void SensorData::enableCalcCompass() { } void SensorData::enableGyroskop() { + this->gyroskop->initialize(); + if (!this->gyroskop->testConnection()) { + std::cout << "SensorData::enableGyroskop: Gyroskop is not conntected. Freeze!" << std::endl; + while (true); + } + + uint8_t deviceStatus = this->gyroskop->dmpInitialize(); + + this->gyroskop->setXGyroOffset(220); + this->gyroskop->setYGyroOffset(76); + this->gyroskop->setZGyroOffset(-85); + this->gyroskop->setZAccelOffset(1788); + if (deviceStatus == 0) { + this->gyroskop->CalibrateAccel(6); + this->gyroskop->CalibrateGyro(6); + this->gyroskop->PrintActiveOffsets(); + this->gyroskop->setDMPEnabled(true); + } else { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + std::cout << "SensorData::enableGyroskop: DMP Initialization failed (code" << (int) deviceStatus <<"). Freeze!" << std::endl; + while (true); + } } CalcAzimuth::State SensorData::getCalcAzimuthState() const { @@ -141,6 +166,12 @@ void SensorData::setOutputStatusPrintPVTdata(bool status) { void SensorData::run() { this->realCompass->read(); this->realAzimuth = this->realCompass->getAzimuth(); + + if (this->gyroskop->dmpGetCurrentFIFOPacket(this->gyroBuffer)) { + this->gyroskop->dmpGetQuaternion(&this->quaternion, this->gyroBuffer); + this->gyroskop->dmpGetGravity(&this->gravity, &this->quaternion); + this->gyroskop->dmpGetYawPitchRoll(this->yawPitchRoll, &this->quaternion, &this->gravity); + } } void SensorData::runAsChild() { diff --git a/lib/Sensors/sensorData.h b/lib/Sensors/sensorData.h index fb4abe1..15add06 100644 --- a/lib/Sensors/sensorData.h +++ b/lib/Sensors/sensorData.h @@ -13,6 +13,7 @@ #define SENSOR_DATA_H #include +#include #include #include "component.h" @@ -20,7 +21,7 @@ #include #include -// Gyroskop +#include #include "calcAzimuth.h" #include "ntripClient.h" @@ -46,11 +47,12 @@ class SensorData : public Component { Point getCurrentPos() const { return this->currentPosition; } const UBX_NAV_PVT_data_t* getGnssData() const { return this->gnssData; }; - const void* const getGyroData() const; + const float* getGyroData() const { return this->yawPitchRoll; } CalcAzimuth* getCalcCompass() const { return this->calcCompass; } QMC5883LCompass* getRealCompass() const { return this->realCompass; } NTRIPClient* getNtripClient() const { return this->ntripClient; } + MPU6050* getGyroskop() const { return this->gyroskop; } // static /** @@ -72,9 +74,12 @@ class SensorData : public Component { CalcAzimuth* calcCompass = nullptr; SFE_UBLOX_GNSS* gnss = nullptr; NTRIPClient* ntripClient = nullptr; + MPU6050* gyroskop = nullptr; UBX_NAV_PVT_data_t* gnssData; Point currentPosition; + Quaternion quaternion; + VectorFloat gravity; char* host; char* mountPoint; @@ -83,18 +88,19 @@ class SensorData : public Component { bool isNtripInit = false; + uint8_t gyroBuffer[64]; uint16_t port; int16_t realAzimuth = INT16_MAX; int16_t calcAzimuth = INT16_MAX; + float yawPitchRoll[3]; + // static static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct); static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct); static UBX_NAV_PVT_data_t* ubxDataStatic; - static uint32_t ubxUpdateTimeStatic; - static bool outputStatusPrintPVTdata; static bool newData; }; diff --git a/platformio.ini b/platformio.ini index da001b9..8009a02 100644 --- a/platformio.ini +++ b/platformio.ini @@ -25,10 +25,8 @@ lib_deps = mprograms/QMC5883LCompass@^1.2.0 https://git.kleiax.de/PlatformIO-Libs/Menu.git nrf24/RF24@^1.4.5 + jrowberg/I2Cdevlib-MPU6050@^1.0.0 upload_port = COM6 -; extra_scripts = -; pre:autoVersionIncrement/version_increment_pre.py -; post:autoVersionIncrement/version_increment_post.py test_ignore = test_desktop build_type = debug monitor_filters = esp32_exception_decoder @@ -37,6 +35,7 @@ check_tool = clangtidy [env:native] platform = native test_ignore = test_embedded +lib_deps = jrowberg/I2Cdevlib-MPU6050@^1.0.0 [platformio] description = A Rover who should be drive a route by gps. diff --git a/src/SpecialMenus/SensorData/menuSensorData.cpp b/src/SpecialMenus/SensorData/menuSensorData.cpp new file mode 100644 index 0000000..8198e4c --- /dev/null +++ b/src/SpecialMenus/SensorData/menuSensorData.cpp @@ -0,0 +1,45 @@ +/** + * @file menuSensorData.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-04 + * + * @copyright Copyright (c) 2023 + * + */ + +#include "menuSensorData.h" + +MenuSensorData::MenuSensorData(SensorData* sensorData) : + MenuInformationSites(4) { + this->sensorData = sensorData; +} + +void MenuSensorData::printPage() const { + String lineOne = ""; + String lineTwo = ""; + + switch (this->getCurrentPage()) { + case 0: + lineOne = "Yaw:"; + lineTwo.concat(this->sensorData->getGyroData()[0]); + break; + + case 1: + lineOne = "Pitch:"; + lineTwo.concat(this->sensorData->getGyroData()[1]); + break; + + case 2: + lineOne = "Roll:"; + lineTwo.concat(this->sensorData->getGyroData()[2]); + break; + + default: + this->printDefault(); + return; + } + + this->print(lineOne, lineTwo); +} diff --git a/src/SpecialMenus/SensorData/menuSensorData.h b/src/SpecialMenus/SensorData/menuSensorData.h new file mode 100644 index 0000000..9f0883b --- /dev/null +++ b/src/SpecialMenus/SensorData/menuSensorData.h @@ -0,0 +1,28 @@ +/** + * @file menuSensorData.h + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-04 + * + * @copyright Copyright (c) 2023 + * + */ + +#ifndef MENU_SENSOR_DATA_H +#define MENU_SENSOR_DATA_H + +#include "menuInformationSites.h" +#include "sensorData.h" + +class MenuSensorData : public MenuInformationSites { + public: + MenuSensorData(SensorData* SensorData); + + private: + void printPage() const override; + + SensorData* sensorData; +}; + +#endif //MENU_SENSOR_DATA_H diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp index a6891ab..a59fa3f 100644 --- a/src/driveModi/Modi/ManualControl/manualControl.cpp +++ b/src/driveModi/Modi/ManualControl/manualControl.cpp @@ -32,7 +32,7 @@ void ManualControl::switchInputMode() { void ManualControl::analogControl() { // Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128) int16_t x = this->input->x - 127; - int16_t y = this-> input->y - 127; + int16_t y = this->input->y - 127; // static int counter = 0; // if (counter % 60 == 0) { diff --git a/src/main.cpp b/src/main.cpp index 15a814c..3f85073 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -46,6 +46,7 @@ #include "SpecialMenus/PID/menuPidSettings.h" #include "SpecialMenus/Route/menuRoute.h" #include "SpecialMenus/GPS/menuGPS.h" +#include "SpecialMenus/SensorData/menuSensorData.h" MoveControl moveController; DriveManager* driveManager; @@ -225,15 +226,18 @@ void makeMenu() { MenuCalibrateCompass* comp_m = new MenuCalibrateCompass(driveManager); MenuGPS* gps_m = new MenuGPS(sensorData); MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery); + MenuSensorData* sen_m = new MenuSensorData(sensorData); //TODO: Wie bekommt jeder die dumme Route? MenuRoute* rout_m = new MenuRoute(new Route()); auto_m->setUpdateDelay(1000); gps_m->setUpdateDelay(1000); sys_m->setUpdateDelay(1500); + sen_m->setUpdateDelay(500); // Entry for the main menu main_m->addEntry(new MenuAction("Mode", mode_m)); + main_m->addEntry(new MenuAction("Sensor", sen_m)); main_m->addEntry(new MenuAction("GPS", gps_m)); main_m->addEntry(new MenuAction("Route", rout_m)); main_m->addEntry(new MenuAction("PID", pid_m));