diff --git a/.vscode/settings.json b/.vscode/settings.json index 1b79192..ba00c90 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -77,7 +77,47 @@ "istream": "cpp", "streambuf": "cpp", "functional": "cpp", - "cmath": "cpp" + "cmath": "cpp", + "atomic": "cpp", + "cctype": "cpp", + "chrono": "cpp", + "clocale": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "iterator": "cpp", + "map": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "limits": "cpp", + "ostream": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "cinttypes": "cpp", + "typeinfo": "cpp" }, "cSpell.words": [ "Ahnung", diff --git a/autoVersionIncrement b/autoVersionIncrement new file mode 160000 index 0000000..fd51b62 --- /dev/null +++ b/autoVersionIncrement @@ -0,0 +1 @@ +Subproject commit fd51b62f000d23d5649da611c2560af8e2327415 diff --git a/doc/Notes and TODOs/SPIpushRawData.txt b/doc/Notes and TODOs/SPIpushRawData.txt deleted file mode 100644 index 3bd1e86..0000000 --- a/doc/Notes and TODOs/SPIpushRawData.txt +++ /dev/null @@ -1,9 +0,0 @@ -Added this code in SparkFun_u-blox_GNSS_Arduino_Libary.cpp in the function pushRawData. - -_spiPort->beginTransaction(SPISettings(_spiSpeed, MSBFIRST, SPI_MODE0)); -digitalWrite(_csPin, LOW); -for (uint16_t i = 0; i < numDataBytes; i++) { - spiTransfer(dataBytes[i]); -} -digitalWrite(_csPin, HIGH); -_spiPort->endTransaction(); diff --git a/doc/Notes and TODOs/ps3 reconnect.txt b/doc/Notes and TODOs/ps3 reconnect.txt deleted file mode 100644 index 6b97f40..0000000 --- a/doc/Notes and TODOs/ps3 reconnect.txt +++ /dev/null @@ -1,26 +0,0 @@ -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(); diff --git a/doc/PinBelegungen.txt b/doc/PinBelegungen.txt deleted file mode 100644 index 9026913..0000000 --- a/doc/PinBelegungen.txt +++ /dev/null @@ -1,30 +0,0 @@ -ESP32 Rover Pinbelegung - - 3V3 GND - x 23 DirR1 - x 22 PWMR - x TX PC - x RX PC Connector I2C -BATTERY 35 21 SDA SDA -INTL1 32 GND GND -INTL2 33 19 SCL SCL -INTR1 25 18 VCC -INTR2 26 5 SPI_CSK -DirL1 27 17 SPI_CS -DirR2 14 16 SPI_COPI -DirL2 12 4 SPI_CIPO - GND x -PWML 13 2 Probleme beim flashen - x 15 WS2812 LED ? - x x - CMD x - 5V USB x - - -Connector Encoder - -- 2 1 + -OB 4 3 - -3V3 6 [5] OA - 8 7 - 10 9 diff --git a/doc/TODO allgemein.txt b/doc/TODO allgemein.txt new file mode 100644 index 0000000..37eaf10 --- /dev/null +++ b/doc/TODO allgemein.txt @@ -0,0 +1,12 @@ +Für irgendwann: + -> Program underfloorLighting + -> Add an beeper + -> Program the beeper + -> Update GNSS Lib to v3 + +Besser zügig: + Speedometer buffer ergibt kaum Sinn + Abweichung max 25 cm bevor zu ungenau damit nie mehr als 50 cm gesamt. + Doxygen Kommentare aktualisieren + Motortreiber vielleicht bei wenigster Leistung ohne Kurve + diff --git a/doc/controller.txt b/doc/controller.txt deleted file mode 100644 index 33966b5..0000000 --- a/doc/controller.txt +++ /dev/null @@ -1,17 +0,0 @@ -Controller - -PS3-Button: Change Mode - -Mode: ManualControl - L3: Joystick Mode - Left Joystick: Drive - - R3: Shoulder Trigger Mode - L2: Spin left side - R2: Spin right side - -Mode: Autopilot - -Mode: CaptureRoute - -Mode: ConsolControl \ No newline at end of file diff --git a/include/Version.h b/include/Version.h new file mode 100644 index 0000000..109f133 --- /dev/null +++ b/include/Version.h @@ -0,0 +1,9 @@ + + // AUTO GENERATED FILE, DO NOT EDIT + #ifndef VERSION + #define VERSION "0.8.36" + #endif + #ifndef BUILD_TIMESTAMP + #define BUILD_TIMESTAMP "2023-08-13 15:13:34.491991" + #endif + \ No newline at end of file diff --git a/include/network.h b/include/network.h index 2a85d24..cf2cd9b 100644 --- a/include/network.h +++ b/include/network.h @@ -16,6 +16,7 @@ #include #include #include +#include #include "networkConfig.h" @@ -44,6 +45,7 @@ class Network { */ static bool connectWifi(); static bool connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send); + static uint8_t getCurrentChannel(); /** * @brief Set all general settings to connect to a broker. diff --git a/lib/MotorControl/motorControl.cpp b/lib/MotorControl/motorControl.cpp index f94111f..96375af 100644 --- a/lib/MotorControl/motorControl.cpp +++ b/lib/MotorControl/motorControl.cpp @@ -17,9 +17,9 @@ MotorControl::MotorControl() { this->setMaxPwm(PWMMAX); } -void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2) { - this->pwm_pin = pwm_pin; - this->pwm_channel = pwm_channel; +void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2) { + this->pwmPin = pwmPin; + this->pwmChannel = pwmChannel; this->dir_1 = dir_1; this->dir_2 = dir_2; @@ -29,17 +29,17 @@ void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uin digitalWrite(this->dir_1, LOW); digitalWrite(this->dir_2, LOW); - ledcSetup(this->pwm_channel, PWMFREQ, this->pwm_res); - ledcAttachPin(this->pwm_pin, this->pwm_channel); - ledcWrite(this->pwm_channel, 0); + ledcSetup(this->pwmChannel, PWMFREQ, this->pwmRes); + ledcAttachPin(this->pwmPin, this->pwmChannel); + ledcWrite(this->pwmChannel, 0); } uint16_t MotorControl::loop() { uint32_t time = millis(); uint16_t elapsed_time = time - this->lastMillis; - //Cancel if delay is not reached - if (elapsed_time < delay) + //Cancel if delayLoop is not reached + if (elapsed_time < delayLoop) return elapsed_time; runMotorControl(); @@ -48,14 +48,14 @@ uint16_t MotorControl::loop() { } void MotorControl::runMotorControl() { - // Absolute difference between target_power and power - uint8_t abs_difference = abs(this->target_power - this->power); + // Absolute difference between targetPower and power + uint8_t abs_difference = abs(this->targetPower - this->power); - // Difference between target_power and power - int16_t difference = this->target_power - this->power; + // Difference between targetPower and power + int16_t difference = this->targetPower - this->power; // Check that the target speed is close to 0 and that the abs_difference is lower than powersteps - if (abs(this->target_power) < powersteps && abs_difference < powersteps) { + if (abs(this->targetPower) < powersteps && abs_difference < powersteps) { this->setRealPower(0); return; } @@ -66,7 +66,7 @@ void MotorControl::runMotorControl() { } // Positive or negative tagret speed - if (this->target_power >= 0) { + if (this->targetPower >= 0) { // Positive or negative speed if (this->power >= 0) { if (difference > 0) { @@ -95,36 +95,36 @@ void MotorControl::runMotorControl() { void MotorControl::setMinPwm(uint8_t min) { if (min > 80) min = 80; //transform percentage to real pwm value - min = (uint8_t) (((1 << pwm_res) - 1) * (min / 100.0)); - this->dutycycle_min = min; + min = (uint8_t) (((1 << pwmRes) - 1) * (min / 100.0)); + this->dutycycleMin = min; } void MotorControl::setMaxPwm(uint8_t max) { if (max > 100) max = 100; //transform percentage to real pwm value - max = (uint8_t) (((1 << pwm_res) - 1) * (max / 100.0)); - this->dutycycle_max = max; + max = (uint8_t) (((1 << pwmRes) - 1) * (max / 100.0)); + this->dutycycleMax = max; } uint16_t MotorControl::setPowerSteps(uint8_t increment) { this->powersteps = increment; - return (uint16_t) (delay * ( 100 / powersteps )); + return (uint16_t) (delayLoop * ( 100 / powersteps )); } void MotorControl::setTargetPower(int8_t power) { if (power <= 100 && power >= -100) - this->target_power = power; + this->targetPower = power; else std::cout << " MotorControl::setTargetPower: Invalid Argument - Power: " << power << std::endl; } -uint16_t MotorControl::setDelay(uint8_t delay) { - this->delay = delay; - return (uint16_t) (delay * ( 100 / powersteps )); +uint16_t MotorControl::setDelay(uint8_t delayLoop) { + this->delayLoop = delayLoop; + return (uint16_t) (delayLoop * ( 100 / powersteps )); } void MotorControl::stop() { - this->target_power = 0; + this->targetPower = 0; } void MotorControl::emergencyStop() { @@ -136,23 +136,23 @@ int8_t MotorControl::getPower() { } int8_t MotorControl::getTargetPower() { - return this->target_power; + return this->targetPower; } bool MotorControl::isTargetPowerReached() { - if (this->target_power == this->power) + if (this->targetPower == this->power) return true; return false; } bool MotorControl::isAccelerationPositive() { - if (power < target_power) + if (power < targetPower) return true; return false; } bool MotorControl::isAccelerationNegative() { - if (power > target_power) + if (power > targetPower) return true; return false; } @@ -169,12 +169,12 @@ void MotorControl::setRealPower(int8_t power) { this->direction = 0; digitalWrite(this->dir_1, LOW); digitalWrite(this->dir_2, LOW); - ledcWrite(this->pwm_channel, 0); + ledcWrite(this->pwmChannel, 0); this->dutycycle = 0; return; } - uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycle_min, this->dutycycle_max); + uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycleMin, this->dutycycleMax); if ((this->direction == 1 || this->direction == 0) && power < 0){ // new direction backward this->direction = 2; @@ -186,7 +186,7 @@ void MotorControl::setRealPower(int8_t power) { digitalWrite(this->dir_2, LOW); } - ledcWrite(this->pwm_channel, pwm_val); + ledcWrite(this->pwmChannel, pwm_val); this->dutycycle = pwm_val; } diff --git a/lib/MotorControl/motorControl.h b/lib/MotorControl/motorControl.h index 4880a42..e5819ed 100644 --- a/lib/MotorControl/motorControl.h +++ b/lib/MotorControl/motorControl.h @@ -21,7 +21,7 @@ #define PWMRES 8 #define POWERSTEPS 2 // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms #define PWMMIN 55 -#define PWMMAX 80 // Max 98% of 2^PWM_RES +#define PWMMAX 94 // Max 98% of 2^PWM_RES /** * @brief A class which use PWM to control the power of DC Motor @@ -35,17 +35,17 @@ class MotorControl { /** * @brief Initialize the motorController * - * @param pwm_pin The output pin for the signal on the esp. - * @param pwm_channel One of the pwm channels from the esp. + * @param pwmPin The output pin for the signal on the esp. + * @param pwmChannel One of the pwm channels from the esp. * @param dir_1 First direction pin for the H-Bridge. * @param dir_2 Second direction pin for the H-Bridge. */ - void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2); + void init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2); /** * @brief Calls runMotorControl() to update the pwm signal * - * This function should be called every mainloop. If the delay is not reached, than the + * This function should be called every mainloop. If the delayLoop is not reached, than the * functions returns immediately. * @see runMotorControl() * @see DELAY @@ -81,10 +81,10 @@ class MotorControl { * * Set the increment of the steps with which the dutycycle is * increased or decreased. Note the dependency between the increment - * and delay(). + * and delayLoop(). * * The formula for the time between 0% and 100% power is: - * time[ms] = delay * ( 100 / increment ) + * time[ms] = delayLoop * ( 100 / increment ) * 500 ms are recommended * * @see setDelay() @@ -105,17 +105,17 @@ class MotorControl { void setTargetPower(int8_t power); /** - * @brief Set the min delay between each loop + * @brief Set the min delayLoop between each loop * - * Note the dependency between delay and + * Note the dependency between delayLoop and * setPowerSteps(). * * @see setPowerSteps() * - * @param delay time in Milliseconds + * @param delayLoop time in Milliseconds * @return time from 0% power to 100% power in Milliseconds */ - uint16_t setDelay(uint8_t delay); + uint16_t setDelay(uint8_t delayLoop); /** * @brief Stops the motor like setTargetPower() to 0 @@ -152,19 +152,19 @@ class MotorControl { void setRealPower(int8_t power); void increasePower(int8_t power); - int8_t target_power = 0; + int8_t targetPower = 0; int8_t power = 0; uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward - uint8_t pwm_pin; - uint8_t pwm_channel; - uint8_t pwm_res = PWMRES; + uint8_t pwmPin; + uint8_t pwmChannel; + uint8_t pwmRes = PWMRES; uint16_t dutycycle = 0; - uint8_t dutycycle_min; - uint8_t dutycycle_max; + uint8_t dutycycleMin; + uint8_t dutycycleMax; uint8_t dir_1; uint8_t dir_2; - uint8_t delay = DELAY; + uint8_t delayLoop = DELAY; uint8_t powersteps = POWERSTEPS; uint32_t lastMillis = 0; diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index cdae8df..e09e260 100644 --- a/lib/Navigation/navigation.cpp +++ b/lib/Navigation/navigation.cpp @@ -98,14 +98,11 @@ void Navigation::loop() { this->ntripClient->loop(); if (millis() - this->lastMillis > AZIMUTH_UPDATE_DELAY) { - // if (millis() - this->lastMillis > 500) { this->compass->read(); - this->azimuth = this->compass->getAzimuth(); - // std::cout << "Compass x: " << compass->getX() - // << " y: " << compass->getY() - // << " z: " << compass->getZ() - // << " Azi: " << compass->getAzimuth() - // << std::endl; + this->realAzimuth = this->compass->getAzimuth(); + + this->updateMagneticDeclination(); + this->lastMillis = millis(); } } @@ -124,6 +121,15 @@ bool Navigation::startNavigation() { return this->navigationStarted; } +void Navigation::drivingDirectionChange() { + Point tmp = this->currentPosition; + if (tmp.isInit() && tmp.isValid()) { + this->directionChangeMode = true; + this->lastPointDrivingDirectionChange = tmp; + this->calcAzimuthState = CalcAzimuthState::Invalid; + } +} + Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) { if (this->navigationFinished) return Status::Complete; @@ -134,6 +140,7 @@ Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0) && !forceUpdate) { correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection); + correction.distance = this->lastPointCalcCorrection.distanceTo(this->targetPoint); return Status::Unchanged; } @@ -189,21 +196,75 @@ void Navigation::updateCurrentLocation() { this->currentPosition = Point(coords, this->ubxData->hAcc); } +void Navigation::updateMagneticDeclination() { + if (!this->directionChangeMode + || this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) < 1.0) + { + this->calcAzimuthState = CalcAzimuthState::Invalid; + this->calcAzimuth = 999; + return; + } + + this->calcAzimuth = this->lastPointDrivingDirectionChange.courseTo(this->currentPosition); + + // Map point accuracy to CalcAzimuthState + if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::oneDigOfCM + || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM) + { + this->calcAzimuthState = CalcAzimuthState::Good; + } + else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::twoDigOfCM + || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM) + { + this->calcAzimuthState = CalcAzimuthState::Ok; + } + else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::threeDigOfCM + || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM) + { + this->calcAzimuthState = CalcAzimuthState::Bad; + } + else + { + this->calcAzimuthState = CalcAzimuthState::Invalid; + } + + // Upgrade quality if the range grows up + if (this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) > 2.0) { + switch (this->calcAzimuthState) { + case CalcAzimuthState::Bad : + this->calcAzimuthState = CalcAzimuthState::Ok; + break; + + case CalcAzimuthState::Ok : + this->calcAzimuthState = CalcAzimuthState::Good; + break; + + case CalcAzimuthState::Good : + this->calcAzimuthState = CalcAzimuthState::Super; + break; + + default: + break; + } + } + +} + int16_t Navigation::calculateCourseCorrection(Point& point) { int16_t targetCourse = point.courseTo(this->targetPoint); - // correction = targetCourse - currentCourse - int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth; - int16_t correctionCourse = targetCourse - signedAzimuth; - if (correctionCourse > 180) - correctionCourse -= 360; - else if (correctionCourse < -180) - correctionCourse += 360; + int16_t correctionCourse; - //Rotate result by 180° to corrigate Azimuth - correctionCourse += 180; - correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse; - - return correctionCourse; + if (this->calcAzimuthState == CalcAzimuthState::Good + || this->calcAzimuthState == CalcAzimuthState::Super) + { + correctionCourse = targetCourse - this->calcAzimuth; + this->lastUsedCalcAzimuth = true; + } else { + correctionCourse = targetCourse - this->realAzimuth; + this->lastUsedCalcAzimuth = false; + } + + return Navigation::fixDegree(correctionCourse); } bool Navigation::nextPoint() { @@ -224,6 +285,16 @@ void Navigation::setOutputStatusPrintPVTdata(bool status) { Navigation::outputStatusPrintPVTdata = status; } +int16_t Navigation::fixDegree(int16_t degree) { + while (degree < -180 || degree > 180) { + if (degree > 180) + degree -= 360; + else if (degree < -180) + degree += 360; + } + return degree; +} + void Navigation::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { if (!Navigation::outputStatusPrintPVTdata) return; diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index 4f2e9ff..d8cc34e 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -62,6 +62,14 @@ class Navigation { Complete }; + enum CalcAzimuthState { + Invalid, + Bad, + Ok, + Good, + Super + }; + /** * @brief Construct a new Navigation object and using I2C @@ -118,6 +126,8 @@ class Navigation { */ bool startNavigation(); void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; }; + void drivingDirectionChange(); + void dissableCalcAzimuth() { this->directionChangeMode = false; } double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; } double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; } @@ -178,15 +188,21 @@ class Navigation { Point getCurrentPosition() const { return this->currentPosition; } /** - * @brief Get azimuth + * @brief Get realAzimuth * * This value represents the angle between north and * the line of sight. Clockwise. * * @return uint16_t degree */ - uint16_t getAzimuth() const { return this->azimuth; } + int16_t getAzimuth() const { return this->realAzimuth; } QMC5883LCompass* getCompass() const { return this->compass; } + int16_t getCalcAzimuth() const { return this->calcAzimuth; } + CalcAzimuthState getCalcAzimuthState() const { return this->calcAzimuthState; } + bool getLastUsedCalcAzimuth() const { return this->lastUsedCalcAzimuth; } + + Point::Accuracy getMinAccuracy() const { return this->minAccuracy; } + void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; } /** * @brief Set the output status for PVTdata. @@ -197,29 +213,16 @@ class Navigation { * @param status */ static void setOutputStatusPrintPVTdata(bool status); + // map input in range from -180 to 180 degree + static int16_t fixDegree(int16_t degree); private: void updateCurrentLocation(); - int16_t calculateCourseCorrection(Point& point); - - /** - * @brief Set the next point as target - * - * @return true - * @return false - */ - bool nextPoint(); - - /** - * @brief Set the target point - * - * @param target - * @return true - * @return false - */ - bool setTargetPoint(Point target); - + void updateMagneticDeclination(); void init(Route* route); + bool nextPoint(); + bool setTargetPoint(Point target); + int16_t calculateCourseCorrection(Point& point); SFE_UBLOX_GNSS* gps; UBX_NAV_PVT_data_t* ubxData = nullptr; @@ -229,24 +232,31 @@ class Navigation { Point lastPointRouteInsert; Point lastPointCalcCorrection; + Point lastPointDrivingDirectionChange; Point targetPoint; Point currentPosition; Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM; + CalcAzimuthState calcAzimuthState = CalcAzimuthState::Invalid; + bool navigationStarted = false; bool navigationFinished = false; bool isNtripInit = false; bool preventNextPoint = false; + bool directionChangeMode = false; + bool lastUsedCalcAzimuth = false; char* host; char* mountPoint; char* user; char* password; + int16_t realAzimuth = INT16_MAX; + int16_t calcAzimuth = INT16_MAX; + uint8_t timeToWait = 200; uint16_t port; - uint16_t azimuth = UINT16_MAX; uint32_t lastMillis = 0; uint32_t ubxUpdateTime = 0; diff --git a/lib/NtripClient/ntripClient.cpp b/lib/NtripClient/ntripClient.cpp index 93d7969..d5cefa1 100644 --- a/lib/NtripClient/ntripClient.cpp +++ b/lib/NtripClient/ntripClient.cpp @@ -221,7 +221,7 @@ void NTRIPClient::closeConnection() { bool NTRIPClient::processConnection() { if (this->ntripClient->connected()) { - uint8_t rtcmData[this->bufferSize * 4]; + uint8_t rtcmData[this->bufferSize * 8]; uint16_t rtcmCount = 0; while (this->ntripClient->available()) { diff --git a/lib/NtripClient/ntripClient.h b/lib/NtripClient/ntripClient.h index 900c6eb..7dc6809 100644 --- a/lib/NtripClient/ntripClient.h +++ b/lib/NtripClient/ntripClient.h @@ -129,7 +129,7 @@ class NTRIPClient { const uint8_t delayTime = 20; const uint8_t maxReconnectAttemps = 10; const uint16_t reconnectDelayTime = 1000; - const uint16_t timeOut = 5000; + const uint16_t timeOut = 10000; const uint16_t bufferSize = 512; const uint16_t pushGPGGATime = 10000; }; diff --git a/lib/Navigation/route.cpp b/lib/Route/route.cpp similarity index 88% rename from lib/Navigation/route.cpp rename to lib/Route/route.cpp index 8d08a78..5f5b395 100644 --- a/lib/Navigation/route.cpp +++ b/lib/Route/route.cpp @@ -11,35 +11,35 @@ #include "route.h" -Point::Point(double lat, double lon, uint32_t horizontalAccuracy) { +Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) { this->coordinates.lat = lat; this->coordinates.lon = lon; - this->init(horizontalAccuracy); + this->init(horizontalAccuracy, creationTime); } -Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy) { +Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime) { this->coordinates.lat = lat / 10000000.0; this->coordinates.lon = lon / 10000000.0; - this->init(horizontalAccuracy); + this->init(horizontalAccuracy, creationTime); } -Point::Point(Coordinates coords, uint32_t horizontalAccuracy) { +Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) { this->coordinates = coords; - this->init(horizontalAccuracy); + this->init(horizontalAccuracy, creationTime); } Point::Point(Coordinates coords, bool imported) { this->coordinates = coords; if (imported) - this->init(UINT32_MAX); + this->init(UINT32_MAX, 0); else - this->init(0); + this->init(0, 0); } Point::Point() { this->coordinates.lat = 0; this->coordinates.lon = 0; - this->init(0); + this->init(0, 0); } bool Point::operator==(const Point& rhs) const { @@ -74,7 +74,7 @@ int16_t Point::courseTo(const Coordinates& point) const { double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) ); double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT); - int16_t res = (int16_t) atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT; + int16_t res = static_cast(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1; // if (res < 0) // res += 360; @@ -86,8 +86,8 @@ int16_t Point::courseTo(const Point &point) const { return this->courseTo(point.getCoordinates()); } -void Point::init(uint32_t horizontalAccuracy) { - this->creationTime = millis(); +void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) { + this->creationTime = creationTime; if (horizontalAccuracy == UINT32_MAX) this->accuracy = Accuracy::imported; diff --git a/lib/Navigation/route.h b/lib/Route/route.h similarity index 96% rename from lib/Navigation/route.h rename to lib/Route/route.h index 62d6c19..e358bfb 100644 --- a/lib/Navigation/route.h +++ b/lib/Route/route.h @@ -12,8 +12,6 @@ #ifndef ROUTE_H #define ROUTE_H -#include - #include #include #include @@ -66,9 +64,9 @@ class Point{ * @param coords Coordinates * @param imported if true than highest accuracy */ - Point(double lat, double lon, uint32_t horizontalAccuracy = 0); - Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0); - Point(Coordinates coords, uint32_t horizontalAccuracy = 0); + Point(double lat, double lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); + Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); + Point(Coordinates coords, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); Point(Coordinates coords, bool imported); Point(); @@ -133,7 +131,7 @@ class Point{ Accuracy getAccuracy() { return this->accuracy; } private: - void init(uint32_t horizontalAccuracy); + void init(uint32_t horizontalAccuracy, uint32_t creationTime); Accuracy accuracy = Accuracy::none; Coordinates coordinates; diff --git a/lib/calibrateCompass/calibrateCompass.cpp b/lib/calibrateCompass/calibrateCompass.cpp index 4d7a1c2..3d17770 100644 --- a/lib/calibrateCompass/calibrateCompass.cpp +++ b/lib/calibrateCompass/calibrateCompass.cpp @@ -73,7 +73,7 @@ void CalibrateCompass::start() { this->clearData(); this->state = State::Calibrating; - this->compass->removeCalibration(); + this->compass->clearCalibration(); this->lastChange = millis(); } @@ -95,7 +95,7 @@ void CalibrateCompass::useData() { } void CalibrateCompass::removeCalibration() { - this->compass->removeCalibration(); + this->compass->clearCalibration(); } void CalibrateCompass::reset() { diff --git a/lib/calibrateCompass/calibrateCompass.h b/lib/calibrateCompass/calibrateCompass.h index 8a8d786..3c5de15 100644 --- a/lib/calibrateCompass/calibrateCompass.h +++ b/lib/calibrateCompass/calibrateCompass.h @@ -52,6 +52,6 @@ class CalibrateCompass { void clearData(); bool dataValid = false; - const uint16_t maxTimeWithoutChange = 5000; + const uint16_t maxTimeWithoutChange = 10000; uint32_t lastChange = 0; }; diff --git a/platformio.ini b/platformio.ini index 3b37405..03e3aa1 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,7 +8,7 @@ ; Please visit documentation for the other options and examples ; https://docs.platformio.org/page/projectconf.html -[env] +[env:embedded] platform = espressif32 board = esp32doit-devkit-v1 board_build.partitions = no_ota.csv @@ -22,18 +22,21 @@ lib_deps = br3ttb/PID@^1.2.1 marcoschwartz/LiquidCrystal_I2C@^1.1.4 bblanchon/ArduinoJson@^6.20.0 - mprograms/QMC5883LCompass@^1.1.1 + mprograms/QMC5883LCompass@^1.2.0 https://git.kleiax.de/PlatformIO-Libs/Menu.git nrf24/RF24@^1.4.5 upload_port = COM3 - -[env:release] -build_type = release - -[env:debug] +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 check_tool = clangtidy +[env:native] +platform = native +test_ignore = test_embedded + [platformio] description = A Rover who should be drive a route by gps. diff --git a/src/SpecialMenus/Route/menuRoute.cpp b/src/SpecialMenus/Route/menuRoute.cpp index c60196b..772c23a 100644 --- a/src/SpecialMenus/Route/menuRoute.cpp +++ b/src/SpecialMenus/Route/menuRoute.cpp @@ -83,18 +83,13 @@ void MenuRoute::init() { this->mainMenu = new Menu; MenuRoutePoints* pointsMenu = new MenuRoutePoints(this->route); - Menu* importMenu = new Menu; - Menu* exportMenu = new Menu; - Menu* clearMenu = new Menu; this->mainMenu->setLcd(this->lcd); pointsMenu->setLcd(this->lcd); - importMenu->setLcd(this->lcd); - exportMenu->setLcd(this->lcd); - clearMenu->setLcd(this->lcd); MenuIntInput* importWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::importRoute)); MenuIntInput* exportWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::exportRoute)); + MenuIntInput* deleteWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::deleteRoute)); MenuActionRoute* clearWrapper = new MenuActionRoute(this, &MenuRoute::clearRoute); importWrapper->setLcd(this->lcd); @@ -107,15 +102,16 @@ void MenuRoute::init() { exportWrapper->setPrintParentMenu(false); exportWrapper->setEntry(0, "Export Route"); - MenuAction* pointsAction = new MenuAction("Points", pointsMenu); - MenuAction* importAction = new MenuAction("Import", importWrapper); - MenuAction* exportAction = new MenuAction("Export", exportWrapper); - MenuAction* clearAction = new MenuAction("Clear", clearWrapper); + deleteWrapper->setLcd(this->lcd); + deleteWrapper->setMinMax(0, 100); + deleteWrapper->setPrintParentMenu(false); + deleteWrapper->setEntry(0, "Delete Route"); - this->mainMenu->addEntry(pointsAction); - this->mainMenu->addEntry(importAction); - this->mainMenu->addEntry(exportAction); - this->mainMenu->addEntry(clearAction); + this->mainMenu->addEntry(new MenuAction("Points", pointsMenu)); + this->mainMenu->addEntry(new MenuAction("Clear", clearWrapper)); + this->mainMenu->addEntry(new MenuAction("Import", importWrapper)); + this->mainMenu->addEntry(new MenuAction("Export", exportWrapper)); + this->mainMenu->addEntry(new MenuAction("Delete", deleteWrapper)); } void MenuRoute::importRoute(uint8_t routeNumber) { @@ -236,6 +232,52 @@ void MenuRoute::exportRoute(uint8_t routeNumber) { http.end(); } +void MenuRoute::deleteRoute(uint8_t routeNumber) { + this->blockInput = true; + + String lineOne = ""; + String lineTwo = ""; + + if (WiFi.status() != WL_CONNECTED) { + lineOne = "Not connected to"; + lineTwo = "the WiFi."; + this->print(lineOne, lineTwo); + this->blockInput = false; + return; + } + + if (ESP.getMaxAllocHeap() < JSON_DOCUMENT_SIZE_ROUTE) { + lineOne = "Not enough mem"; + lineTwo = "for Json obj"; + this->print(lineOne, lineTwo); + this->blockInput = false; + return; + } + + WiFiClient client; + HTTPClient http; + DynamicJsonDocument doc(JSON_DOCUMENT_SIZE_ROUTE); + + String host = "http://rover.kleiax.de/api/"; + host.concat(routeNumber); + http.begin(client, host); + int httpResponseCode = http.sendRequest("DELETE"); + + lineOne = "Delete complete"; + lineTwo = "Code: "; + + if (httpResponseCode == 202) + deserializeJson(doc, http.getStream()); + else + lineOne = "HTTP Error"; + + lineTwo.concat(httpResponseCode); + this->print(lineOne, lineTwo); + + this->blockInput = false; + http.end(); +} + void MenuRoute::clearRoute(uint8_t none) { this->route->clear(); this->print("Currente route", "deleted..."); diff --git a/src/SpecialMenus/Route/menuRoute.h b/src/SpecialMenus/Route/menuRoute.h index ebfc50e..b8cdae8 100644 --- a/src/SpecialMenus/Route/menuRoute.h +++ b/src/SpecialMenus/Route/menuRoute.h @@ -109,6 +109,8 @@ class MenuRoute : public MenuControl { */ void exportRoute(uint8_t routeNumber); + void deleteRoute(uint8_t routeNumber); + /** * @brief Delete the current Route. * diff --git a/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp b/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp index 44793a2..fd2e5ab 100644 --- a/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp +++ b/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp @@ -12,7 +12,7 @@ #include "menuSysteminformation.h" MenuSysteminformation::MenuSysteminformation(Battery* mainBattery) - : MenuInformationSites(5) { + : MenuInformationSites(8) { this->mainBattery = mainBattery; } @@ -44,6 +44,22 @@ void MenuSysteminformation::printPage() const { break; case 4: + lineOne = "Current WiFi"; + lineTwo = "channel: "; + lineTwo.concat(Network::getCurrentChannel()); + break; + + case 5: + lineOne = "Software verion:"; + lineTwo = VERSION; + break; + + case 6: + lineOne = "Build timestamp:"; + lineTwo = String(BUILD_TIMESTAMP).substring(0, 16); + break; + + case 7: lineOne = "Kleiax Rover by"; lineTwo = "Alexander Klein"; break; diff --git a/src/SpecialMenus/Systeminformation/menuSysteminformation.h b/src/SpecialMenus/Systeminformation/menuSysteminformation.h index 2a046ab..d0d885a 100644 --- a/src/SpecialMenus/Systeminformation/menuSysteminformation.h +++ b/src/SpecialMenus/Systeminformation/menuSysteminformation.h @@ -14,9 +14,12 @@ #include +#include "Version.h" + #include "controlPadInput.h" #include "menuInformationSites.h" #include "battery.h" +#include "network.h" /** * @brief Prints information about the current system status. diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp index 4f7ac06..c7f4f1f 100644 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp +++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp @@ -43,7 +43,7 @@ void MenuAutopilot::printPage() const { lineTwo = ""; switch (this->autopilot->getState()) { case Autopilot::State::InsufficientAccuarcy : - lineTwo = "Err: No NTRIP"; + lineTwo = "Err: LowAccuracy"; break; case Autopilot::State::NoRoute : @@ -66,6 +66,10 @@ void MenuAutopilot::printPage() const { lineTwo = "Autopilot active"; break; + case Autopilot::State::SelfDrivingRotate : + lineTwo = "Rotating"; + break; + case Autopilot::State::TargetReached : lineTwo = "Target reached"; break; @@ -158,10 +162,69 @@ void MenuAutopilot::printPage() const { lineTwo.concat(" - Decrease"); break; + case 10: + lineOne = "Current minimal"; + if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM) + lineTwo = "accuracy is high"; + else + lineTwo = "accuracy is low"; + break; + + case 11: + lineOne = "CalcAzi: "; + lineTwo = "State: "; + lineOne.concat(this->driveManager->getNavigation()->getCalcAzimuth()); + switch (this->driveManager->getNavigation()->getCalcAzimuthState()) { + case Navigation::CalcAzimuthState::Bad : + lineTwo.concat("Bad"); + break; + + case Navigation::CalcAzimuthState::Good : + lineTwo.concat("Good"); + break; + + case Navigation::CalcAzimuthState::Invalid : + lineTwo.concat("Invalid"); + break; + + case Navigation::CalcAzimuthState::Ok : + lineTwo.concat("Ok"); + break; + + case Navigation::CalcAzimuthState::Super : + lineTwo.concat("Super"); + break; + + default: + lineTwo.concat("Unkown"); + break; + } + break; + + case 12: + lineOne = "Test rotate"; + lineTwo = "180 degree"; + break; + + case 13: + lineOne = "Test rotate"; + lineTwo = "270 degree"; + break; + + case 14: + lineOne = "Test rotate"; + lineTwo = "45 degree"; + break; + + case 15: + lineOne = "Test rotate"; + lineTwo = "20 degree"; + break; + default: this->printDefault(); return; - } + } this->print(lineOne, lineTwo); } @@ -189,6 +252,29 @@ void MenuAutopilot::runCommand() const { case 9: this->minDistance = this->driveManager->getNavigation()->decreaseMinDistanceToReachPoint(); break; + + case 10: + if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM) + this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::none); + else + this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM); + break; + + case 12: + this->autopilot->testRotate(180); + break; + + case 13: + this->autopilot->testRotate(270); + break; + + case 14: + this->autopilot->testRotate(45); + break; + + case 15: + this->autopilot->testRotate(20); + break; default: break; @@ -204,7 +290,7 @@ void MenuAutopilot::update() { } void MenuAutopilot::init() { - this->setCountPages(10); + this->setCountPages(16); this->driveManager->changeModus(Modi::Autopilot); this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr(); this->routeInfo = this->autopilot->getRouteInfo(); diff --git a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp index 82c18f1..013a5f5 100644 --- a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp +++ b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp @@ -77,20 +77,25 @@ void MenuCalibrateCompass::printPage() const { break; case 7: + lineOne = "Reset for new"; + lineTwo = "calibration run"; + break; + + case 8: lineOne = "X min: "; lineOne.concat(this->caliCompass->getCallibrationData().data[0][0]); lineTwo = "X max: "; lineTwo.concat(this->caliCompass->getCallibrationData().data[0][1]); break; - case 8: + case 9: lineOne = "Y min: "; lineOne.concat(this->caliCompass->getCallibrationData().data[1][0]); lineTwo = "Y max: "; lineTwo.concat(this->caliCompass->getCallibrationData().data[1][1]); break; - case 9: + case 10: lineOne = "Z min: "; lineOne.concat(this->caliCompass->getCallibrationData().data[2][0]); lineTwo = "Z max: "; @@ -109,7 +114,7 @@ void MenuCalibrateCompass::init() { this->firstPrint = false; this->driveManager->changeModus(Modi::ManualControl); this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr(); - this->setCountPages(10); + this->setCountPages(11); this->updateDelay = 500; } @@ -125,7 +130,6 @@ void MenuCalibrateCompass::runCommand() const { case CalibrateCompass::State::Finished: this->manualControl->setCalibrateCompass(); this->caliCompass->useData(); - this->caliCompass->reset(); break; default: @@ -147,7 +151,11 @@ void MenuCalibrateCompass::runCommand() const { case 6: this->caliCompass->useData(); - break; + break; + + case 7: + this->caliCompass->reset(); + break; default: break; diff --git a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp index 8d6846f..94da0a9 100644 --- a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp +++ b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp @@ -91,6 +91,14 @@ void MenuCaptureRoute::printPage() const { else lineOne.concat("0"); break; + + case 7: + lineOne = "Current minimal"; + if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM) + lineTwo = "accuracy is high"; + else + lineTwo = "accuracy is low"; + break; default: this->printDefault(); @@ -106,8 +114,23 @@ void MenuCaptureRoute::update() { this->printMenu(); } +void MenuCaptureRoute::runCommand() const { + switch (this->getCurrentPage()) + { + case 7: + if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM) + this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::none); + else + this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM); + break; + + default: + break; + } +} + void MenuCaptureRoute::init() { - this->setCountPages(7); + this->setCountPages(8); this->updateDelay = 500; this->driveManager->changeModus(Modi::CaptureRoute); this->captureRoute = (CaptureRoute*) this->driveManager->getDriveModiPtr(); diff --git a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h index c29dfe0..3f321ea 100644 --- a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h +++ b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h @@ -42,6 +42,8 @@ class MenuCaptureRoute : public MenuDriveMode { */ void update() override; + void runCommand() const override; + private: void init() override; diff --git a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp index dffebd8..e122de6 100644 --- a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp +++ b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp @@ -27,44 +27,66 @@ void MenuManualControl::printPage() const { break; case 1: - lineOne = "Speed: "; - lineOne.concat(this->manualControl->getMaxSpeed()); - lineTwo = "Increase by 0.1"; + lineOne = "Input mode:"; + if (this->manualControl->getInputMode() == ManualControl::InputMode::Analog) + lineTwo = "Analog"; + else + lineTwo = "Digital"; break; case 2: lineOne = "Speed: "; lineOne.concat(this->manualControl->getMaxSpeed()); - lineTwo = "Decrease by 0.1"; + lineTwo = "Increase by 0.1"; break; case 3: - lineOne = "RotSpeed: "; - lineOne.concat(this->manualControl->getMaxRotation()); - lineTwo = "Increase by 0.1"; + lineOne = "Speed: "; + lineOne.concat(this->manualControl->getMaxSpeed()); + lineTwo = "Decrease by 0.1"; break; case 4: lineOne = "RotSpeed: "; lineOne.concat(this->manualControl->getMaxRotation()); - lineTwo = "Decrease by 0.1"; + lineTwo = "Increase by 0.1"; break; case 5: + lineOne = "RotSpeed: "; + lineOne.concat(this->manualControl->getMaxRotation()); + lineTwo = "Decrease by 0.1"; + break; + + case 6: lineOne = "Dutycycle Left:"; lineTwo.concat(this->manualControl->getDutycycleLeft()); break; - case 6: + case 7: lineOne = "Dutycycle Right:"; lineTwo.concat(this->manualControl->getDutycycleRight()); break; - case 7: + case 8: lineOne = "Azimuth:"; lineTwo.concat(this->driveManager->getNavigation()->getAzimuth()); break; + case 9: { + UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData(); + lineOne = "magDec: "; + lineTwo = "magAcc: "; + if (gpsData->valid.bits.validMag) { + lineOne.concat(gpsData->magDec); + lineTwo.concat(gpsData->magAcc); + } else { + lineOne.concat("invalid"); + lineTwo.concat("invalid"); + } + break; + } + default: this->printDefault(); return; @@ -77,28 +99,31 @@ void MenuManualControl::init() { this->firstPrint = false; this->driveManager->changeModus(Modi::ManualControl); this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr(); - this->setCountPages(8); + this->setCountPages(10); this->updateDelay = 500; } void MenuManualControl::runCommand() const { switch (this->getCurrentPage()) { - case 1: - this->manualControl->increaseMaxSpeed(); + this->manualControl->switchInputMode(); break; case 2: - this->manualControl->decreaseMaxSpeed(); + this->manualControl->increaseMaxSpeed(); break; case 3: - this->manualControl->increaseMaxRotation(); + this->manualControl->decreaseMaxSpeed(); break; case 4: this->manualControl->increaseMaxRotation(); break; + + case 5: + this->manualControl->increaseMaxRotation(); + break; default: break; diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp index 46efabe..004138f 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/src/driveModi/Modi/Autopilot/autopilot.cpp @@ -12,13 +12,31 @@ #include "driveModi/Modi/Autopilot/autopilot.h" #include "autopilot.h" +DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) { + this->navigation = navigation; + this->action(); +} + +DirectionChangeSignal::~DirectionChangeSignal() { + navigation->dissableCalcAzimuth(); +} + +void DirectionChangeSignal::action() { + this->navigation->drivingDirectionChange(); +} + + Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation) : ManualControl(moveControl, input) { + this->setInputMode(ManualControl::InputMode::Digital); + this->directionChangeSignal = new DirectionChangeSignal(navigation); + this->setDirectionChangeCallback(this->directionChangeSignal); this->navigation = navigation; this->init(); } Autopilot::~Autopilot() { + delete this->directionChangeSignal; // this->navigation->getNTRIPClient()->setActivated(false); } @@ -53,6 +71,7 @@ void Autopilot::runAutopilot() { case State::NavigationStarted: this->askNavigationForOrder(); + this->checkButtonInput(); break; case State::GetToStartPoint: @@ -72,6 +91,11 @@ void Autopilot::runAutopilot() { this->selfDriving(); break; + case SelfDrivingRotate: + this->checkButtonInput(); + this->rotate(); + break; + case State::TargetReached: break; @@ -92,6 +116,14 @@ bool Autopilot::shouldUpdate() { return false; } +void Autopilot::testRotate(int16_t degree) { + if (!degree) + return; + + this->courseCorrection.correction = degree; + this->beginRotate(); +} + void Autopilot::init() { if (this->navigation->startNavigation()) this->state = State::NavigationStarted; @@ -115,28 +147,62 @@ void Autopilot::drive() { this,moveControl->setSpeed(0); } -void Autopilot::rotate() { - this->moveControl->setSpeed(0); - if (this->courseCorrection.correction > 0) - this->moveControl->setRotationSpeed(this->rotationSpeed); - else +void Autopilot::beginRotate() { + if (this->state != State::SelfDrivingRotate) { + this->lastState = this->state; + this->state = State::SelfDrivingRotate; + this->rotationAimAzimuth = this->navigation->getAzimuth() + this->courseCorrection.correction; + this->rotationAimAzimuth = Navigation::fixDegree(this->rotationAimAzimuth); - this->moveControl->setRotationSpeed(-this->rotationSpeed); + this->moveControl->setSpeed(0); + if (this->courseCorrection.correction > 0) + this->moveControl->setRotationSpeed(-this->rotationSpeed); + else + this->moveControl->setRotationSpeed(this->rotationSpeed); + } +} + +void Autopilot::rotate() { + if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 5) { + this->endRotate(); + return; + } + + if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 15) + && + ((this->courseCorrection.correction > 0 + && this->rotationAimAzimuth < this->navigation->getAzimuth()) + || (this->courseCorrection.correction < 0 + && this->rotationAimAzimuth > this->navigation->getAzimuth()))) + { + this->endRotate(); + std::cout << "Autopilot::rotate: Rover rotated too far" << std::endl; + } +} + +void Autopilot::endRotate() { + if (this->state != State::SelfDrivingRotate) + return; + + this->state = this->lastState; + this->navigation->drivingDirectionChange(); + this->moveControl->setRotationSpeed(0); + this->moveControl->emergencyStop(); + this->moveControl->setDrivingStatus(MoveControl::Status::Drive); } void Autopilot::checkButtonInput() { if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action) && millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) { - if (this->state == State::SelfDrivingAvailable) { + if (this->state == State::SelfDrivingAvailable) this->state = State::SelfDriving; - this->updateDisplay = true; - this->lastAutopilotChangeMillis = millis(); - } else if (this->state == State::SelfDriving) { + else if (this->state == State::SelfDriving || this->state == State::SelfDrivingRotate) this->state = State::SelfDrivingAvailable; - this->updateDisplay = true; - this->lastAutopilotChangeMillis = millis(); - } + else if (this->state == State::NavigationStarted) + this->state = State::GetToStartPoint; + this->updateDisplay = true; + this->lastAutopilotChangeMillis = millis(); } } @@ -172,10 +238,11 @@ void Autopilot::askNavigationForOrder() { } void Autopilot::selfDriving() { - if (this->lastOrderStatus == Navigation::Status::Updated) { - if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct) - this->rotate(); - else - this->drive(); - } + if (this->state != State::SelfDriving) + return; + + if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct) + this->beginRotate(); + else + this->drive(); } diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h index 4cd124b..2fd53c5 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.h +++ b/src/driveModi/Modi/Autopilot/autopilot.h @@ -17,6 +17,16 @@ #include "moveControl.h" #include "driveModi/Modi/ManualControl/manualControl.h" +class DirectionChangeSignal : public DirectionChangeWrapper { + public: + DirectionChangeSignal(Navigation* navigation); + ~DirectionChangeSignal(); + void action() override; + + private: + Navigation* navigation; +}; + /** * @brief This class use the navigate class to drive automaticaly * @@ -36,6 +46,7 @@ class Autopilot : public ManualControl { GetToStartPoint, SelfDrivingAvailable, SelfDriving, + SelfDrivingRotate, TargetReached }; @@ -66,15 +77,6 @@ class Autopilot : public ManualControl { */ void loop(); - /** - * @brief Manges the autopilot - * - * If the first Point is near to current location you can turn - * the autopilot on. - * Gets the course correction and decide what to do. - */ - void runAutopilot(); - void restart(); /** @@ -105,11 +107,15 @@ class Autopilot : public ManualControl { * @return false */ bool shouldUpdate(); + void testRotate(int16_t degree); + void endRotate(); private: void init(); void drive(); + void beginRotate(); void rotate(); + void runAutopilot(); void checkButtonInput(); void askNavigationForOrder(); void selfDriving(); @@ -120,6 +126,7 @@ class Autopilot : public ManualControl { State state = State::None; State lastState = State::None; Navigation::Status lastOrderStatus; + DirectionChangeSignal* directionChangeSignal; bool updateDisplay = false; @@ -131,8 +138,10 @@ class Autopilot : public ManualControl { uint32_t loopLastMillis = 0; uint32_t lastAutopilotChangeMillis = 0; + int16_t rotationAimAzimuth; + double drivingSpeed = 1; - double rotationSpeed = 3; + double rotationSpeed = 4.5; double minRemainingDistance = 0.25; }; diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp index 5ce8dc9..0a9858c 100644 --- a/src/driveModi/Modi/ManualControl/manualControl.cpp +++ b/src/driveModi/Modi/ManualControl/manualControl.cpp @@ -26,14 +26,30 @@ void ManualControl::loop() { if (this->caliCompass) this->caliCompass->loop(); - if (millis() - this->lastMillis < delay) { + if (millis() - this->lastMillis < delay) return; + + switch (this->inputMode) { + case InputMode::Analog : + this->analogControl(); + break; + + case InputMode::Digital : + this->digitalControl(); + break; + + default: + break; } - this->runManualControl(); + this->lastMillis = millis(); } -void ManualControl::runManualControl() { +void ManualControl::switchInputMode() { + this->inputMode = (this->inputMode == InputMode::Analog) ? InputMode::Digital : InputMode::Analog; +} + +void ManualControl::analogControl() { // Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128) int16_t y = this->input->x - 127; int16_t x = this->input->y - 127; @@ -49,3 +65,30 @@ void ManualControl::runManualControl() { value_per_step = this->max_rotation * 2 / UINT8_MAX; this->moveControl->setRotationSpeed(x * value_per_step); } + +void ManualControl::digitalControl() { + int16_t y = this->input->x - 127; + int16_t x = this->input->y - 127; + + if (y > 120) + this->moveControl->setSpeed(-this->max_speed); + else if (y < -120) + this->moveControl->setSpeed(this->max_speed); + else + this->moveControl->setSpeed(0); + + if (x > 120) + this->moveControl->setRotationSpeed(this->max_rotation); + else if (x < -120) + this->moveControl->setRotationSpeed(-this->max_rotation); + else + this->moveControl->setRotationSpeed(0); + + if (this->directionChangeWrapper && (x > 120 || x < -120)) + this->lastLoopTurned = true; + else if (this->lastLoopTurned) { + if (this->directionChangeWrapper) + this->directionChangeWrapper->action(); + this->lastLoopTurned = false; + } +} diff --git a/src/driveModi/Modi/ManualControl/manualControl.h b/src/driveModi/Modi/ManualControl/manualControl.h index d64062b..35d242d 100644 --- a/src/driveModi/Modi/ManualControl/manualControl.h +++ b/src/driveModi/Modi/ManualControl/manualControl.h @@ -16,6 +16,11 @@ #include "controlPadInput.h" #include "calibrateCompass.h" +class DirectionChangeWrapper { + public: + virtual void action() = 0; +}; + /** * @brief Drive the Rover with a Joystick * @@ -26,6 +31,11 @@ */ class ManualControl : public DriveModi { public: + enum class InputMode : uint8_t { + Analog, + Digital + }; + ManualControl(MoveControl *moveControl, const ControlPadInput *input); /** @@ -35,7 +45,7 @@ class ManualControl : public DriveModi { ~ManualControl(); /** - * @brief Calls runManualControl() to update all values. + * @brief Calls analogControl() to update all values. * * This function should be called every mainloop. If the delay is not reached, than the * functions returns immediately. @@ -44,12 +54,6 @@ class ManualControl : public DriveModi { */ void loop() override; - /** - * @brief Noramly called repeatedly by loop() to calcluate new values. - * Set new values for speed and rotation in moveControl - */ - void runManualControl(); - /** * @brief Set the min delay between each loop * @@ -79,6 +83,11 @@ class ManualControl : public DriveModi { void setCalibrateCompass(CalibrateCompass* val = nullptr) { this->caliCompass = val; } + void switchInputMode(); + void setInputMode(InputMode mode) { this->inputMode = mode; } + InputMode getInputMode() const { return this->inputMode; } + void setDirectionChangeCallback(DirectionChangeWrapper* callback) { this->directionChangeWrapper = callback; } + uint16_t getDutycycleLeft() const { return this->moveControl->getDutycycleLeft(); } uint16_t getDutycycleRight() const { return this->moveControl->getDutycycleRight(); } @@ -87,11 +96,21 @@ class ManualControl : public DriveModi { const ControlPadInput* input; private: - uint32_t lastMillis = 0; + /** + * @brief Noramly called repeatedly by loop() to calcluate new values. + * Set new values for speed and rotation in moveControl + */ + void analogControl(); + void digitalControl(); + + bool lastLoopTurned = false; uint8_t delay = 10; + uint32_t lastMillis = 0; double max_speed = 1; double max_rotation = 7; CalibrateCompass* caliCompass = nullptr; + DirectionChangeWrapper* directionChangeWrapper = nullptr; + InputMode inputMode = InputMode::Analog; }; #endif // MANUAL_CONTROL_H diff --git a/src/main.cpp b/src/main.cpp index a02a11b..77a5668 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,6 +22,8 @@ #include #include +#include "Version.h" + #include "driveModi/driveManager.h" #include "OutputBuf/outputBuf.h" #include "LcdWrapper.h" @@ -109,6 +111,8 @@ void setup() { Network::connectEspNow(receiveCallback, sendCallback); std::cout << "Welcome to Kleiax-Rover" << std::endl; + std::cout << "Project verion: " << VERSION << std::endl; + std::cout << "Build timestamp: " << BUILD_TIMESTAMP << std::endl; std::cout << "All actions from the main program run on Core -> " << xPortGetCoreID() << std::endl; driveManager = new DriveManager(&moveController, spiPort, controlPad->getControlPadDataPtr(), wifiIsActive); @@ -118,7 +122,7 @@ void setup() { lcd->backlight(); lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator); lcd->setCursor(0, 1); - lcd->print("Press PS-Button"); + lcd->printf("WiFi channel %u", Network::getCurrentChannel()); lcdWrapper = new LcdWrapper(lcd); lcdWrapper->setCallback(lcdWrapperCallback); @@ -231,41 +235,25 @@ void makeMenu() { sys_m->setUpdateDelay(1500); rout_m->setLcd(lcdWrapper); - - // Entry for the main menu - MenuAction* mode_e = new MenuAction("Mode", mode_m); - MenuAction* gps_e = new MenuAction("GPS", gps_m); - MenuAction* pid_e = new MenuAction("PID", pid_m); - MenuAction* restart_e = new MenuAction("Restart", restart); - MenuAction* sys_e = new MenuAction("Systeminfo", sys_m); - MenuAction* rout_e = new MenuAction("Route", rout_m); - main_m->addEntry(mode_e); - main_m->addEntry(gps_e); - main_m->addEntry(rout_e); - main_m->addEntry(pid_e); - main_m->addEntry(sys_e); - main_m->addEntry(restart_e); + main_m->addEntry(new MenuAction("Mode", mode_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)); + main_m->addEntry(new MenuAction("Systeminfo", sys_m)); + main_m->addEntry(new MenuAction("Restart", restart)); // Entry for the mode Menu - MenuAction* autopilot_e = new MenuAction("Autopilot", auto_m); - MenuAction* captureRoute_e = new MenuAction("Capture Route", cap_m); - MenuAction* consolControl_e = new MenuAction("Consol Control", dummy); - MenuAction* manualControl_e = new MenuAction("Manual Control", man_m); - MenuAction* testMode_e = new MenuAction("Test Mode", testM_m); - MenuAction* caliComp_e = new MenuAction("Gauge Compass", comp_m); - mode_m->addEntry(manualControl_e); - mode_m->addEntry(captureRoute_e); - mode_m->addEntry(autopilot_e); - mode_m->addEntry(caliComp_e); - mode_m->addEntry(testMode_e); - mode_m->addEntry(consolControl_e); + mode_m->addEntry(new MenuAction("Manual Control", man_m)); + mode_m->addEntry(new MenuAction("Capture Route", cap_m)); + mode_m->addEntry(new MenuAction("Autopilot", auto_m)); + mode_m->addEntry(new MenuAction("Gauge Compass", comp_m)); + mode_m->addEntry(new MenuAction("Test Mode", testM_m)); + mode_m->addEntry(new MenuAction("Consol Control", dummy)); // Entry for the PID Menu - MenuAction* pidl_e = new MenuAction("Left", pidl_m); - MenuAction* pidr_e = new MenuAction("Right", pidr_m); - pid_m->addEntry(pidl_e); - pid_m->addEntry(pidr_e); + pid_m->addEntry(new MenuAction("Left", pidl_m)); + pid_m->addEntry(new MenuAction("Right", pidr_m)); // Other config pidl_m->setMinMax(0, UINT8_MAX); diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 48090c6..a8bc80d 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -187,9 +187,9 @@ void MoveControl::calcTargetWheelSpeed() { \ 1 -b / \ T / \ Xr / */ // (1 / r) * 1 - const static double A = 15.82278481; + constexpr double A = 15.82278481; // (1 / r) * b - const static double B = 2.096518987; + constexpr double B = 2.096518987; this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (WHEEL_DIAMETER / 2); this->wheelspeed_left_target = (A * this->x_speed + (-B) * this->rotation_speed) * (WHEEL_DIAMETER / 2); diff --git a/src/network.cpp b/src/network.cpp index 8d1a1e0..baaa932 100644 --- a/src/network.cpp +++ b/src/network.cpp @@ -113,6 +113,19 @@ bool Network::connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send) { return true; } +uint8_t Network::getCurrentChannel() { + uint8_t channel; + wifi_second_chan_t secondChannel; + if (esp_wifi_get_channel(&channel, &secondChannel) != ESP_OK) { + std::cout << "Network::getCurrentChannel - Error!" << std::endl; + return -1; + } + // std::cout << "Network::getCurrentChannel - Current WiFi channel: " + // << (int) channel << " second channel: " << (int) secondChannel + // << std::endl; + return channel; +} + bool Network::checkMQTT() { static uint64_t lastReconnectAttempt = 0; if (!mqtt_client->connected()) { diff --git a/test/test_desktop/bootstrap.cpp b/test/test_desktop/bootstrap.cpp new file mode 100644 index 0000000..1bbb8aa --- /dev/null +++ b/test/test_desktop/bootstrap.cpp @@ -0,0 +1,22 @@ +#include "courseCalculation.hpp" +#include + +void setUp() { + +} + +void tearDown() { + +} + +void uselessTest() { + TEST_ASSERT_EQUAL(12, 12); +} + +int main(int argc, char **argv) { + UNITY_BEGIN(); + RUN_TEST(testCourseCalculation); + UNITY_END(); + + return 0; +} diff --git a/test/test_desktop/courseCalculation.hpp b/test/test_desktop/courseCalculation.hpp new file mode 100644 index 0000000..4950cc0 --- /dev/null +++ b/test/test_desktop/courseCalculation.hpp @@ -0,0 +1,15 @@ +#include "route.h" +#include +#include + +void testCourseCalculation(void) { + // Point pointA(53.106725, 7.248069); // 45 + // Point pointA(53.112188, 7.264404); // 90 + Point pointA(53.112072, 7.266619); // -90 + Point pointB(53.112123, 7.265354); //Home + int res = pointA.courseTo(pointB); + std::cout << "" << std::endl; + std::cout << "" << std::endl; + std::cout << "Return Value: " << res << std::endl; + TEST_ASSERT_LESS_OR_EQUAL(10, res); +} diff --git a/version b/version new file mode 100644 index 0000000..98bb028 --- /dev/null +++ b/version @@ -0,0 +1 @@ +0.8.36 \ No newline at end of file