kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

clangtidy corrections part 2

Browse Source
This commit is contained in:
kleiax
2023-10-12 00:44:27 +02:00
parent 23d854a826
commit a43e2db92b
41 changed files with 2274 additions and 2485 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/MQTT/debugMqtt.cpp
+16 -14
View File
@@ -18,10 +18,12 @@ char DebugMqtt::msg[MQTT_BUFFER_SIZE];
char DebugMqtt::topic[MQTT_BUFFER_SIZE];
DebugMqtt::DebugMqtt(const char* name, uint8_t bufSize) {
this->name = name;
if (bufSize != 0)
DebugMqtt::DebugMqtt(const char* name, uint8_t bufSize)
: name {name}
{
if (bufSize != 0) {
this->bufSize = bufSize;
}
this->buf = new char[this->bufSize];
}
@@ -30,8 +32,8 @@ DebugMqtt::~DebugMqtt() {
}
void DebugMqtt::sendMsg(Loglevel loglevel, String topic, String msg) {
snprintf (DebugMqtt::msg, MQTT_BUFFER_SIZE, "%s: %s",this->name ,msg.c_str());
this->sendData(loglevel, topic, DebugMqtt::msg);
snprintf (static_cast<char*>(DebugMqtt::msg), MQTT_BUFFER_SIZE, static_cast<const char*>("%s: %s"),this->name ,msg.c_str());
this->sendData(loglevel, topic, static_cast<const char*>(DebugMqtt::msg));
}
void DebugMqtt::sendMsg(Loglevel loglevel, String msg) {
@@ -44,27 +46,27 @@ void DebugMqtt::sendData(Loglevel loglevel, String topic, String data) {
}
if (loglevel <= DebugMqtt::loglevel && loglevel > Loglevel::none) {
snprintf (DebugMqtt::topic, MQTT_BUFFER_SIZE, "%s%s", DebugMqtt::enum_to_string(loglevel).c_str(), topic.c_str());
snprintf (DebugMqtt::msg, MQTT_BUFFER_SIZE, "%s", data.c_str());
client->publish(DebugMqtt::topic, DebugMqtt::msg);
snprintf (static_cast<char*>(DebugMqtt::topic), MQTT_BUFFER_SIZE, static_cast<const char*>("%s%s"), DebugMqtt::enum_to_string(loglevel).c_str(), topic.c_str());
snprintf (static_cast<char*>(DebugMqtt::msg), MQTT_BUFFER_SIZE, static_cast<const char*>("%s"), data.c_str());
client->publish(DebugMqtt::topic, static_cast<const char*>(DebugMqtt::msg));
}
}
void DebugMqtt::sendData(Loglevel loglevel, String data){
this->sendData(loglevel, "", data);
DebugMqtt::sendData(loglevel, "", data);
}
void DebugMqtt::writeToInflux(String measurement_name, String field_set, float measurement, uint64_t nanos) {
// Example String: "weather temperature=82 1465839830100400200";
snprintf(DebugMqtt::msg, MQTT_BUFFER_SIZE, "%s %s=%f %llu", measurement_name.c_str(), field_set.c_str(), measurement, nanos);
this->sendData(Loglevel::influx, DebugMqtt::msg);
snprintf(static_cast<char*>(DebugMqtt::msg), MQTT_BUFFER_SIZE, static_cast<const char*>("%s %s=%f %llu"), measurement_name.c_str(), field_set.c_str(), measurement, nanos);
this->sendData(Loglevel::influx, static_cast<const char*>(DebugMqtt::msg));
}
void DebugMqtt::addCharacter(char c) {
this->buf[this->bufPos] = c;
void DebugMqtt::addCharacter(char character) {
this->buf[this->bufPos] = character;
this->bufPos++;
if (c == '\n' || this->bufPos >= this->bufSize - 1) {
if (character == '\n' || this->bufPos >= this->bufSize - 1) {
this->buf[this->bufPos - 1] = '\0';
this->sendMsg(Loglevel::info, buf);
this->bufPos = 0;
lib/MQTT/debugMqtt.h
+4 -4
View File
@@ -108,8 +108,8 @@ class DebugMqtt {
* @param topic Additional topic behind loglevel
* @param data The message to send as String
*/
void sendData(Loglevel loglevel, String topic, String data);
void sendData(Loglevel loglevel, String data);
static void sendData(Loglevel loglevel, String topic, String data);
static void sendData(Loglevel loglevel, String data);
/**
* @brief Send a Message via MQTT for InfluxDB
@@ -132,9 +132,9 @@ class DebugMqtt {
* 1. when the buffer is full
* 2. when the character is '\n'
*
* @param c
* @param character
*/
void addCharacter(char c);
void addCharacter(char character);
/**
* @brief Initialize debugMQTT for all instances
lib/MotorControl/motorControl.cpp
+96 -57
View File
@@ -5,20 +5,20 @@
* @see motorControl.h
* @version 0.1
* @date 2021-12-13
*
*
* @copyright Copyright (c) 2021
*
*
*/
#include "motorControl.h"
MotorControl::MotorControl() {
this->setMinPwm(MotorControl::pwmMin);
this->setMaxPwm(MotorControl::pwmMax);
MotorControl::MotorControl()
{
Component::loopDelay = MotorControl::loopDelay;
}
void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2) {
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;
@@ -35,107 +35,141 @@ void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8
ledcWrite(this->pwmChannel, 0);
}
void MotorControl::run() {
void MotorControl::run()
{
// Absolute difference between targetPower and power
uint8_t abs_difference = abs(this->targetPower - this->power);
const uint8_t abs_difference = abs(this->targetPower - this->power);
// Difference between targetPower and power
int16_t difference = this->targetPower - this->power;
const int16_t difference = this->targetPower - this->power;
// Check that the target speed is close to 0 and that the abs_difference is lower than MotorControl::powerSteps
if (abs(this->targetPower) < MotorControl::powerSteps && abs_difference < MotorControl::powerSteps) {
if (abs(this->targetPower) < MotorControl::powerSteps && abs_difference < MotorControl::powerSteps)
{
this->setRealPower(0);
return;
}
// Correct speed
if (abs_difference < MotorControl::powerSteps) {
if (abs_difference < MotorControl::powerSteps)
{
return;
}
// Positive or negative tagret speed
if (this->targetPower >= 0) {
if (this->targetPower >= 0)
{
// Positive or negative speed
if (this->power >= 0) {
if (difference > 0) {
if (this->power >= 0)
{
if (difference > 0)
{
this->increasePower(MotorControl::powerSteps);
} else {
}
else
{
this->increasePower(-MotorControl::powerSteps);
}
} else {
}
else
{
this->increasePower(MotorControl::powerSteps);
}
} else {
}
else
{
// Positive or negative speed
if (this->power >= 0) {
if (this->power >= 0)
{
this->increasePower(-MotorControl::powerSteps);
} else {
if (difference > 0) {
}
else
{
if (difference > 0)
{
this->increasePower(MotorControl::powerSteps);
} else {
}
else
{
this->increasePower(-MotorControl::powerSteps);
}
}
}
}
void MotorControl::setMinPwm(uint8_t min) {
if (min > 80) min = 80;
//transform percentage to real pwm value
min = (uint8_t) (((1 << pwmRes) - 1) * (min / 100.0));
void MotorControl::setMinPwm(uint8_t min)
{
if (min > MotorControl::maxPwmMin)
{
min = MotorControl::maxPwmMin;
}
// transform percentage to real pwm value
min = static_cast<uint8_t>(((static_cast<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 << pwmRes) - 1) * (max / 100.0));
void MotorControl::setMaxPwm(uint8_t max)
{
if (max > 100)
{
max = 100;
}
// transform percentage to real pwm value
max = static_cast<uint8_t>(((static_cast<uint8_t>(1) << pwmRes) - 1) * (max / 100.0));
this->dutycycleMax = max;
}
void MotorControl::setTargetPower(int8_t power) {
void MotorControl::setTargetPower(int8_t power)
{
if (power <= 100 && power >= -100)
{
this->targetPower = power;
}
else
{
std::cout << " MotorControl::setTargetPower: Invalid Argument - Power: " << power << std::endl;
}
}
void MotorControl::stop() {
void MotorControl::stop()
{
this->targetPower = 0;
}
void MotorControl::emergencyStop() {
void MotorControl::emergencyStop()
{
setRealPower(0);
}
bool MotorControl::isTargetPowerReached() const {
if (this->targetPower == this->power)
return true;
return false;
bool MotorControl::isTargetPowerReached() const
{
return this->targetPower == this->power;
}
bool MotorControl::isAccelerationPositive() const {
if (power < targetPower)
return true;
return false;
bool MotorControl::isAccelerationPositive() const
{
return power < targetPower;
}
bool MotorControl::isAccelerationNegative() const {
if (power > targetPower)
return true;
return false;
bool MotorControl::isAccelerationNegative() const
{
return power > targetPower;
}
void MotorControl::setRealPower(int8_t power) {
//TODO: Exceptionhandling
if (power <= 100 && power >= -100) {
void MotorControl::setRealPower(int8_t power)
{
// TODO: Exceptionhandling
if (power <= 100 && power >= -100)
{
this->power = power;
} else {
}
else
{
return;
}
if (this->power == 0) {
if (this->power == 0)
{
this->direction = 0;
digitalWrite(this->dir_1, LOW);
digitalWrite(this->dir_2, LOW);
@@ -144,13 +178,16 @@ void MotorControl::setRealPower(int8_t power) {
return;
}
uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycleMin, this->dutycycleMax);
const 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
if ((this->direction == 1 || this->direction == 0) && power < 0)
{ // new direction backward
this->direction = 2;
digitalWrite(this->dir_1, LOW);
digitalWrite(this->dir_2, HIGH);
} else if ((this->direction == 2 || this->direction == 0) && power > 0){ // new direction forward
}
else if ((this->direction == 2 || this->direction == 0) && power > 0)
{ // new direction forward
this->direction = 1;
digitalWrite(this->dir_1, HIGH);
digitalWrite(this->dir_2, LOW);
@@ -160,10 +197,12 @@ void MotorControl::setRealPower(int8_t power) {
this->dutycycle = pwm_val;
}
void MotorControl::increasePower(int8_t power) {
//TODO: Exceptionhandling
//TODO: make a stop befor a direction change
if (abs(power) > 2 * MotorControl::powerSteps) {
void MotorControl::increasePower(int8_t power)
{
// TODO: Exceptionhandling
// TODO: make a stop befor a direction change
if (abs(power) > 2 * MotorControl::powerSteps)
{
Serial.println("Invalid Argument in MotorControl::increasePower");
return;
}
lib/MotorControl/motorControl.h
+81 -81
View File
@@ -4,9 +4,9 @@
* @brief Inherits a class to control a motor with pwm signal.
* @version 0.1
* @date 2021-12-09
*
*
* @copyright Copyright (c) 2021
*
*
*/
#ifndef MOTOR_CONTROL_H
#define MOTOR_CONTROL_H
@@ -23,98 +23,98 @@
* You can control the acceleration of the motor, for example to
* prevent a damage on your H-Bridge.
*/
class MotorControl : public Component {
public:
MotorControl();
class MotorControl : public Component
{
public:
MotorControl();
/**
* @brief Initialize the motorController
*
* @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 pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2);
/**
* @brief Initialize the motorController
*
* @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 pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2);
/**
* @brief Set the minimum duty cycle
*
* @param min duty cycle in percent
*/
void setMinPwm(uint8_t min);
/**
* @brief Set the minimum duty cycle
*
* @param min duty cycle in percent
*/
void setMinPwm(uint8_t min);
/**
* @brief Set the maximum duty cycle
*
* @param max duty cycle in percent
*/
void setMaxPwm(uint8_t max);
/**
* @brief Set the maximum duty cycle
*
* @param max duty cycle in percent
*/
void setMaxPwm(uint8_t max);
/**
* @brief Set the Target Power
*
* If the given power is greater than 100 or smaller than -100, then
* this function only print an error to consol.
*
* @param power power in percent
*/
void setTargetPower(int8_t power);
/**
* @brief Set the Target Power
*
* If the given power is greater than 100 or smaller than -100, then
* this function only print an error to consol.
*
* @param power power in percent
*/
void setTargetPower(int8_t power);
/**
* @brief Stops the motor like setTargetPower() to 0
*
*/
void stop();
/**
* @brief Stops the motor like setTargetPower() to 0
*
*/
void stop();
/**
* @brief Stops the motor immediately
*
*/
void emergencyStop();
/**
* @brief Stops the motor immediately
*
*/
void emergencyStop();
/**
* @brief Get the current power
*
* @return int8_t percent of power (-100 to 100)
*/
int8_t getPower() const { return this->power; };
/**
* @brief Get the current power
*
* @return int8_t percent of power (-100 to 100)
*/
int8_t getPower() const { return this->power; };
/**
* @brief Get the target power
*
* @return int8_t percent of power (-100 to 100)
*/
int8_t getTargetPower() const { return this->targetPower; };
/**
* @brief Get the target power
*
* @return int8_t percent of power (-100 to 100)
*/
int8_t getTargetPower() const { return this->targetPower; };
uint16_t getDutycycle() const { return this->dutycycle; }
bool isTargetPowerReached() const;
bool isAccelerationPositive() const;
bool isAccelerationNegative() const;
uint16_t getDutycycle() const { return this->dutycycle; }
bool isTargetPowerReached() const;
bool isAccelerationPositive() const;
bool isAccelerationNegative() const;
private:
void run() override;
void setRealPower(int8_t power);
void increasePower(int8_t power);
private:
void run() override;
void setRealPower(int8_t power);
void increasePower(int8_t power);
static constexpr uint8_t loopDelay = 10;
static constexpr uint16_t pwmFreq = 16000;
static constexpr uint8_t pwmRes = 8;
static constexpr uint8_t powerSteps = 2; // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
static constexpr uint8_t pwmMin = 55;
static constexpr uint8_t pwmMax = 98; // Max 98% of 2^PWM_RES
static constexpr uint8_t loopDelay = 10;
static constexpr uint16_t pwmFreq = 16000;
static constexpr uint8_t pwmRes = 8;
static constexpr uint8_t powerSteps = 2; // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
static constexpr uint8_t maxPwmMin = 80;
int8_t targetPower = 0;
int8_t power = 0;
uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward
int8_t targetPower = 0;
int8_t power = 0;
uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward
uint8_t pwmPin;
uint8_t pwmChannel;
uint16_t dutycycle = 0;
uint8_t dutycycleMin;
uint8_t dutycycleMax;
uint8_t dir_1;
uint8_t dir_2;
uint8_t pwmPin = 0;
uint8_t pwmChannel = 0;
uint16_t dutycycle = 0;
uint8_t dutycycleMin = 55;
uint8_t dutycycleMax = 98; // Max 98% of 2^PWM_RES
uint8_t dir_1 = 0;
uint8_t dir_2 = 0;
};
#endif // MOTOR_CONTROL_H
lib/Navigation/navigation.cpp
+75 -52
View File
@@ -4,56 +4,72 @@
* @brief Contains the implementation of the class Navigation
* @version 0.1
* @date 2022-01-31
*
*
* @copyright Copyright (c) 2022
*
*
*/
#include "navigation.h"
Navigation::Navigation(const SensorData* sensorData, Route* route) {
this->sensorData = sensorData;
Navigation::Navigation(const SensorData *sensorData, Route *route)
: sensorData{sensorData}
{
this->init(route);
}
void Navigation::init(Route* route) {
if (route)
void Navigation::init(Route *route)
{
if (static_cast<bool>(route))
{
this->route = route;
}
else
{
this->route = new Route();
}
Component::loopDelay = Navigation::loopDelay;
}
Navigation::~Navigation() {
Navigation::~Navigation()
{
delete this->route;
}
void Navigation::run() {}
void Navigation::newRoute() {
if (this->route)
delete this->route;
void Navigation::newRoute()
{
delete this->route;
this->route = new Route();
}
bool Navigation::startNavigation() {
Point newTargetPoint = this->route->startRoute();
bool Navigation::startNavigation()
{
const Point newTargetPoint = this->route->startRoute();
this->navigationStarted = this->setTargetPoint(newTargetPoint);
if (this->navigationStarted)
{
this->navigationFinished = false;
}
return this->navigationStarted;
}
Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) {
Navigation::Status Navigation::getCourseCorrection(CourseCorrection &correction, bool forceUpdate)
{
if (this->navigationFinished)
{
return Status::Complete;
}
if (this->currentPosition.getAccuracy() <= this->minAccuracy)
{
return Status::InsufficientAccuracy;
}
if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0)
&& !forceUpdate) {
// Check if the Rover has moved, if the Rover hasnt moved this function will be return.
if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint) / 2 && !forceUpdate)
{
correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection);
correction.distance = this->lastPointCalcCorrection.distanceTo(this->targetPoint);
return Status::Unchanged;
@@ -62,8 +78,10 @@ Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction,
double distance = this->currentPosition.distanceTo(this->targetPoint);
// Check if I need a new Point
if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false) {
if (!this->nextPoint()) {
if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false)
{
if (!this->nextPoint())
{
this->navigationFinished = true;
this->navigationStarted = false;
return Status::Complete; // End of navigation
@@ -77,21 +95,25 @@ Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction,
return Status::Updated;
}
Navigation::Status Navigation::addCurrentPosToRoute() {
Navigation::Status Navigation::addCurrentPosToRoute()
{
if (this->currentPosition.getAccuracy() <= this->minAccuracy)
{
return Status::InsufficientAccuracy;
}
// First Point
if (this->route->getRouteInfo().totalPoints == 0) {
if (this->route->getRouteInfo().totalPoints == 0)
{
this->route->addPointToRoute(this->currentPosition);
this->lastPointRouteInsert = this->currentPosition;
return Status::Updated;
}
// Every Point after the first
double distance = this->currentPosition.distanceTo(this->lastPointRouteInsert);
if (Navigation::minDisBetweenPoints <= distance
&& Navigation::maxDisBetweenPoints >= distance){
const double distance = this->currentPosition.distanceTo(this->lastPointRouteInsert);
if (Navigation::minDisBetweenPoints <= distance && Navigation::maxDisBetweenPoints >= distance)
{
this->route->addPointToRoute(this->currentPosition);
this->lastPointRouteInsert = this->currentPosition;
return Status::Updated;
@@ -99,57 +121,58 @@ Navigation::Status Navigation::addCurrentPosToRoute() {
return Status::Unchanged;
}
// void Navigation::updateCurrentLocation() {
// if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime)
// return;
int16_t Navigation::calculateCourseCorrection(Point &point)
{
const int16_t targetCourse = point.courseTo(this->targetPoint);
int16_t correctionCourse = 0;
// this->ubxData = Navigation::ubxDataStatic;
// this->ubxUpdateTime = Navigation::ubxUpdateTimeStatic;
// Point::Coordinates coords;
// coords.lat = this->ubxData->lat / 10000000.0;
// coords.lon = this->ubxData->lon / 10000000.0;
// this->currentPosition = Point(coords, this->ubxData->hAcc);
// }
int16_t Navigation::calculateCourseCorrection(Point& point) {
int16_t targetCourse = point.courseTo(this->targetPoint);
int16_t correctionCourse;
if (this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Good
|| this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Super)
if (this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Good || this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Super)
{
correctionCourse = targetCourse - this->sensorData->getCalcAzimuth();
this->lastUsedCalcAzimuth = true;
} else {
}
else
{
correctionCourse = targetCourse - this->sensorData->getRealAzimuth();
this->lastUsedCalcAzimuth = false;
}
return Navigation::fixDegree(correctionCourse);
}
bool Navigation::nextPoint() {
bool Navigation::nextPoint()
{
if (!this->navigationStarted)
{
return false;
}
return this->setTargetPoint(this->route->getNextPoint());
}
bool Navigation::setTargetPoint(Point target) {
if (target.isInit()) {
bool Navigation::setTargetPoint(Point target)
{
if (target.isInit())
{
this->targetPoint = target;
return true;
}
return false;
}
int16_t Navigation::fixDegree(int16_t degree) {
while (degree < -180 || degree > 180) {
if (degree > 180)
degree -= 360;
else if (degree < -180)
degree += 360;
int16_t Navigation::fixDegree(int16_t degree)
{
static constexpr uint16_t fullCircle = 360;
while (degree < -fullCircle / 2 || degree > fullCircle / 2)
{
if (degree > fullCircle / 2)
{
degree -= fullCircle;
}
else if (degree < -fullCircle / 2)
{
degree += fullCircle;
}
}
return degree;
}
lib/Navigation/navigation.h
+112 -111
View File
@@ -4,9 +4,9 @@
* @brief Contains a class which navigate an object by the given route
* @version 0.1
* @date 2022-01-10
*
*
* @copyright Copyright (c) 2022
*
*
*/
#ifndef NAVIGATION_H
@@ -21,143 +21,144 @@
/**
* @brief This struct inherits the result of the navigation
*
*
* The drive get objects of this struct and should
* correct the direction in dependency on this.
*
*
*/
struct CourseCorrection {
struct CourseCorrection
{
int16_t correction;
double distance;
};
/**
* @brief This class navigate an object
*
*
* The class use the given Route and the gps device
* to tell the driver in which direction he have to
* be drive and the distance to the next checkpoint.
*
*
*/
class Navigation : public Component {
public:
enum Status {
InsufficientAccuracy,
Unchanged,
Updated,
Complete
};
class Navigation : public Component
{
public:
enum Status
{
InsufficientAccuracy,
Unchanged,
Updated,
Complete
};
/**
* @brief Construct a new Navigation object and using I2C
*
* @param route with which to navigate
*/
Navigation(const SensorData* sensorData, Route* route = nullptr);
/**
* @brief Construct a new Navigation object and using I2C
*
* @param route with which to navigate
*/
Navigation(const SensorData *sensorData, Route *route = nullptr);
/**
* @brief Destroy the Navigation object
*
*/
~Navigation();
/**
* @brief Destroy the Navigation object
*
*/
~Navigation();
/**
* @brief creates a new empty route
*
*/
void newRoute();
/**
* @brief creates a new empty route
*
*/
void newRoute();
/**
* @brief Tries to start the route
*
* For example the route can not be started
* if there are no Points or wrong Points.
*
* @return true route is started
* @return false route can not be started
*/
bool startNavigation();
void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };
double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }
/**
* @brief Tries to start the route
*
* For example the route can not be started
* if there are no Points or wrong Points.
*
* @return true route is started
* @return false route can not be started
*/
bool startNavigation();
void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };
// TODO: Dokumentation korrigieren.
/**
* @brief Get the Course Correction object
*
* This should be called by the driver to get new instructions.
*
* @param correction passed as refernce to get the data
* @return true if new correction data provided
* @return false if route is finished
*/
Status getCourseCorrection(CourseCorrection& correction, bool forceUpdate = false);
double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }
// TODO: Dokumentation korrigieren.
/**
* @brief Get the Course Correction object
*
* This should be called by the driver to get new instructions.
*
* @param correction passed as refernce to get the data
* @return true if new correction data provided
* @return false if route is finished
*/
Status getCourseCorrection(CourseCorrection &correction, bool forceUpdate = false);
// TODO: Dokumentation korrigieren.
/**
* @brief Tries to add the current Position to the route
*
* This can be go wrong if there is no valid GPS signal
*
* @return true successful added point
* @return false no point added to route
*/
Status addCurrentPosToRoute();
// TODO: Dokumentation korrigieren.
/**
* @brief Tries to add the current Position to the route
*
* This can be go wrong if there is no valid GPS signal
*
* @return true successful added point
* @return false no point added to route
*/
Status addCurrentPosToRoute();
/**
* @brief Get the Route Info object
*
* This object contains information about the route.
* For example the stored points.
*
* @return RouteInfo
*/
RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); }
Route* getRoute() const { return this->route; }
Point getCurrentPosition() const { return this->currentPosition; }
/**
* @brief Get the Route Info object
*
* This object contains information about the route.
* For example the stored points.
*
* @return RouteInfo
*/
RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); }
Route *getRoute() const { return this->route; }
Point getCurrentPosition() const { return this->currentPosition; }
Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; }
Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; }
// map input in range from -180 to 180 degree
static int16_t fixDegree(int16_t degree);
// map input in range from -180 to 180 degree
static int16_t fixDegree(int16_t degree);
private:
void run() override;
void init(Route* route);
bool nextPoint();
bool setTargetPoint(Point target);
int16_t calculateCourseCorrection(Point& point);
private:
void run() override;
void init(Route *route);
bool nextPoint();
bool setTargetPoint(Point target);
int16_t calculateCourseCorrection(Point &point);
static constexpr uint8_t loopDelay = 20;
static constexpr uint8_t maxDisBetweenPoints = 10;
static constexpr float minDisBetweenPoints = 0.3;
const SensorData* sensorData;
Route* route = nullptr;
static constexpr uint8_t loopDelay = 20;
static constexpr uint8_t maxDisBetweenPoints = 10;
static constexpr float minDisBetweenPoints = 0.3;
Point lastPointRouteInsert;
Point lastPointCalcCorrection;
Point lastPointDrivingDirectionChange;
Point targetPoint;
Point currentPosition;
Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
bool navigationStarted = false;
bool navigationFinished = false;
bool isNtripInit = false;
bool preventNextPoint = false;
bool directionChangeMode = false;
bool lastUsedCalcAzimuth = false;
const SensorData *sensorData;
Route *route = nullptr;
uint8_t timeToWait = 200;
uint32_t lastMillis = 0;
uint32_t ubxUpdateTime = 0;
Point lastPointRouteInsert;
Point lastPointCalcCorrection;
Point lastPointDrivingDirectionChange;
Point targetPoint;
Point currentPosition;
Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
double minDistanceToReachPoint = 0.5;
bool navigationStarted = false;
bool navigationFinished = false;
bool isNtripInit = false;
bool preventNextPoint = false;
bool directionChangeMode = false;
bool lastUsedCalcAzimuth = false;
uint8_t timeToWait = 200;
uint32_t lastMillis = 0;
uint32_t ubxUpdateTime = 0;
double minDistanceToReachPoint = 0.5;
};
#endif // NAVIGATION_H
lib/Network/network.cpp
+79 -45
View File
@@ -1,33 +1,38 @@
/**
* @file network.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief
* @version 0.1
* @date 2023-09-18
*
*
* @copyright Copyright (c) 2023
*
*
*/
#include "network.h"
Network::Network(const char *ssid, const char *passphrase) {
if (!WiFi.mode(WIFI_AP_STA))
Network::Network(const char *ssid, const char *passphrase)
{
if (!WiFiGenericClass::mode(WIFI_AP_STA))
{
std::cout << "Network::connectWiFi failed WiFi.mode" << std::endl;
}
this->init(ssid, passphrase);
}
Network::Network(const char *ssid, const char *passphrase, NetworkAdresses adresses) {
this->adresses = adresses;
if (!WiFi.mode(WIFI_AP_STA))
Network::Network(const char *ssid, const char *passphrase, NetworkAdresses adresses)
: adresses{adresses}
{
if (!WiFiGenericClass::mode(WIFI_AP_STA))
{
std::cout << "Network::connectWiFi failed WiFi.mode" << std::endl;
}
if (!WiFi.config(this->adresses.localIP,
this->adresses.gateway,
this->adresses.subnet,
this->adresses.dnsServer))
this->adresses.dnsServer))
{
std::cout << "STA Failed to configure" << std::endl;
}
@@ -35,13 +40,15 @@ Network::Network(const char *ssid, const char *passphrase, NetworkAdresses adres
this->init(ssid, passphrase);
}
Network::~Network() {
if (this->mqttClient)
delete mqttClient;
Network::~Network()
{
delete mqttClient;
}
bool Network::activateEspNow(recieveCallbackPtr reci, sendCallbackPtr send) {
if (esp_now_init() != ESP_OK) {
bool Network::activateEspNow(recieveCallbackPtr reci, sendCallbackPtr send)
{
if (esp_now_init() != ESP_OK)
{
std::cout << "Network::activateEspNow - Error initializing ESP-NOW" << std::endl;
return false;
}
@@ -49,11 +56,12 @@ bool Network::activateEspNow(recieveCallbackPtr reci, sendCallbackPtr send) {
esp_now_register_send_cb(send);
esp_now_peer_info_t peerInfo = {};
memcpy(peerInfo.peer_addr, this->broadcastAddress, 6);
peerInfo.channel = 0;
memcpy(static_cast<void *>(peerInfo.peer_addr), static_cast<const void *>(this->broadcastAddress), 6);
peerInfo.channel = 0;
peerInfo.encrypt = false;
if (esp_now_add_peer(&peerInfo) != ESP_OK) {
if (esp_now_add_peer(&peerInfo) != ESP_OK)
{
std::cout << "Network::connectEspNow - Failed to add peer" << std::endl;
return false;
}
@@ -62,7 +70,8 @@ bool Network::activateEspNow(recieveCallbackPtr reci, sendCallbackPtr send) {
return true;
}
bool Network::activateMqtt(const char *user, const char *passphrase) {
bool Network::activateMqtt(const char *user, const char *passphrase)
{
this->mqttUser = user;
this->mqttPassphrase = passphrase;
@@ -71,53 +80,68 @@ bool Network::activateMqtt(const char *user, const char *passphrase) {
this->mqttClient->setSocketTimeout(1);
if (this->wifiConnected)
{
return this->connectMqtt();
}
return false;
}
void Network::printIPs() {
const void Network::printIPs()
{
std::cout << std::endl;
if (!this->wifiConnected) {
if (!this->wifiConnected)
{
std::cout << "WiFi is not connected." << std::endl;
return;
}
std::cout << "WiFi is connected to" << std::endl;
std::cout << "IP address: " << std::endl;
std::cout << WiFi.localIP().toString().c_str() << std::endl;
std::cout << "WiFi MAC Address: " << WiFi.macAddress().c_str() << std::endl << std::endl;
std::cout << "WiFi MAC Address: " << WiFi.macAddress().c_str() << std::endl
<< std::endl;
}
uint8_t Network::getCurrentChannel() {
uint8_t channel;
wifi_second_chan_t secondChannel;
if (esp_wifi_get_channel(&channel, &secondChannel) != ESP_OK) {
uint8_t Network::getCurrentChannel()
{
uint8_t channel = 0;
wifi_second_chan_t secondChannel = WIFI_SECOND_CHAN_NONE;
if (esp_wifi_get_channel(&channel, &secondChannel) != ESP_OK)
{
std::cout << "Network::getCurrentChannel - Error!" << std::endl;
return -1;
}
return channel;
}
void Network::runAsChild() {
void Network::runAsChild()
{
if (!this->initSucessful)
{
return;
}
this->checkWifi();
if (this->wifiConnected && this->mqttClient)
if (this->wifiConnected && static_cast<bool>(this->mqttClient))
{
this->checkMqtt();
}
}
void Network::init(const char *ssid, const char *passphrase) {
void Network::init(const char *ssid, const char *passphrase)
{
// Connect to Wi-Fi network with SSID and password
std::cout << "Connecting to " << ssid << std::endl;
WiFi.begin(ssid, passphrase);
uint8_t timeout = Network::wifiConnectTimeout;
while (WiFi.status() != WL_CONNECTED) {
while (WiFiSTAClass::status() != WL_CONNECTED)
{
delay(Network::wifiConnectLoopTime);
std::cout << "." << std::flush;
timeout--;
if (timeout == 0) {
if (timeout == 0)
{
std::cout << std::endl;
std::cout << "WiFi NOT connected." << std::endl;
return;
@@ -128,9 +152,9 @@ void Network::init(const char *ssid, const char *passphrase) {
this->printIPs();
}
void Network::checkWifi() {
if ((WiFi.status() != WL_CONNECTED)
&& (millis() - this->lastWifiRecoonectAttemp >= Network::wifiReconnectDelay))
void Network::checkWifi()
{
if ((WiFiSTAClass::status() != WL_CONNECTED) && (millis() - this->lastWifiRecoonectAttemp >= Network::wifiReconnectDelay))
{
std::cout << "Reconnecting to WiFi..." << std::endl;
WiFi.disconnect();
@@ -139,29 +163,39 @@ void Network::checkWifi() {
}
}
void Network::checkMqtt() {
if (!this->mqttClient->connected()
&& millis() - this->lastMqttReconnectAttemp > Network::mqttReconnectDelay)
void Network::checkMqtt()
{
if (!this->mqttClient->connected() && millis() - this->lastMqttReconnectAttemp > Network::mqttReconnectDelay)
{
this->mqttConnected = this->connectMqtt();
this->lastMqttReconnectAttemp = millis();
}
if (this->mqttConnected)
{
this->mqttClient->loop();
}
}
bool Network::connectMqtt() {
bool Network::connectMqtt()
{
String clientId = "ESP32Rover-";
clientId += String(random(0xffff), HEX);
clientId += String(random(), HEX);
if (this->mqttUser) {
if (static_cast<bool>(this->mqttUser))
{
if (this->mqttClient->connect(clientId.c_str(), this->mqttUser, this->mqttPassphrase))
{
this->mqttClient->publish("Rover/Info", "Connected to Mqtt-Broker");
} else {
if (this->mqttClient->connect(clientId.c_str()))
this->mqttClient->publish("Rover/Info", "Connected to Mqtt-Broker");
}
}
else
{
if (this->mqttClient->connect(clientId.c_str()))
{
this->mqttClient->publish("Rover/Info", "Connected to Mqtt-Broker");
}
}
return this->mqttClient->connected();
}
lib/Network/network.h
+44 -43
View File
@@ -1,12 +1,12 @@
/**
* @file network.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief
* @version 0.1
* @date 2023-09-18
*
*
* @copyright Copyright (c) 2023
*
*
*/
#ifndef NETWORK_H
@@ -20,10 +20,11 @@
#include <esp_now.h>
#include <esp_wifi.h>
typedef void (*recieveCallbackPtr) (const uint8_t * mac, const uint8_t *incomingData, int len);
typedef void (*sendCallbackPtr) (const uint8_t *mac_addr, esp_now_send_status_t status);
typedef void (*recieveCallbackPtr)(const uint8_t *mac, const uint8_t *incomingData, int len);
typedef void (*sendCallbackPtr)(const uint8_t *mac_addr, esp_now_send_status_t status);
struct NetworkAdresses {
struct NetworkAdresses
{
IPAddress localIP;
IPAddress gateway;
IPAddress subnet;
@@ -32,53 +33,53 @@ struct NetworkAdresses {
uint16_t mqttPort = 1883;
};
class Network : public Component {
public:
Network(const char *ssid, const char *passphrase);
Network(const char *ssid, const char *passphrase, NetworkAdresses adresses);
class Network : public Component
{
public:
Network(const char *ssid, const char *passphrase);
Network(const char *ssid, const char *passphrase, NetworkAdresses adresses);
~Network();
~Network();
bool activateEspNow(recieveCallbackPtr reci, sendCallbackPtr send);
bool activateMqtt(const char *user = nullptr, const char *passphrase = nullptr);
bool activateEspNow(recieveCallbackPtr reci, sendCallbackPtr send);
bool activateMqtt(const char *user = nullptr, const char *passphrase = nullptr);
void printIPs();
const void printIPs();
bool isWifiConnected() const { return this->wifiConnected; }
bool isMqttConnected() const { return this->mqttConnected; }
const uint8_t* getBroadcastAddress() const { return this->broadcastAddress; }
PubSubClient* getMqttClient() const { return this->mqttClient; }
bool isWifiConnected() const { return this->wifiConnected; }
bool isMqttConnected() const { return this->mqttConnected; }
const uint8_t *getBroadcastAddress() const { return this->broadcastAddress; }
PubSubClient *getMqttClient() const { return this->mqttClient; }
static uint8_t getCurrentChannel();
private:
void run() override {};
void runAsChild() override;
void init(const char *ssid, const char *passphrase);
void checkWifi();
void checkMqtt();
bool connectMqtt();
static uint8_t getCurrentChannel();
NetworkAdresses adresses;
WiFiClient wifiClient;
PubSubClient* mqttClient = nullptr;
private:
void run() override{};
void runAsChild() override;
void init(const char *ssid, const char *passphrase);
void checkWifi();
void checkMqtt();
bool connectMqtt();
bool initSucessful = false;
bool wifiConnected = false;
bool mqttConnected = false;
NetworkAdresses adresses;
WiFiClient wifiClient;
PubSubClient *mqttClient = nullptr;
const char *mqttUser;
const char *mqttPassphrase;
bool initSucessful = false;
bool wifiConnected = false;
bool mqttConnected = false;
uint8_t broadcastAddress[6] = {0xC8, 0xC9, 0xA3, 0xC8, 0x57, 0x10};
uint32_t lastWifiRecoonectAttemp = 0;
uint32_t lastMqttReconnectAttemp = 0;
const char *mqttUser = nullptr;
const char *mqttPassphrase = nullptr;
static constexpr uint8_t wifiConnectTimeout = 20;
static constexpr uint16_t wifiConnectLoopTime = 500;
static constexpr uint16_t wifiReconnectDelay = 5000;
static constexpr uint16_t mqttReconnectDelay = 2500;
uint8_t broadcastAddress[6] = {0xC8, 0xC9, 0xA3, 0xC8, 0x57, 0x10};
uint32_t lastWifiRecoonectAttemp = 0;
uint32_t lastMqttReconnectAttemp = 0;
static constexpr uint8_t wifiConnectTimeout = 20;
static constexpr uint16_t wifiConnectLoopTime = 500;
static constexpr uint16_t wifiReconnectDelay = 5000;
static constexpr uint16_t mqttReconnectDelay = 2500;
};
#endif //NETWORK_H
#endif // NETWORK_H
lib/NtripClient/ntripClient.cpp
+192 -134
View File
@@ -4,84 +4,90 @@
* @brief Contains the implementation of the class NTRIPClient.
* @version 0.1
* @date 2022-09-18
*
*
* @copyright Copyright (c) 2022
*
*
*/
#include "ntripClient.h"
NTRIPClient::NTRIPClient(SFE_UBLOX_GNSS* gps, const char* host, uint16_t port, const char* mountPoint, const char* user, const char* password) {
this->gps = gps;
strcpy(this->host, host);
this->port = port;
strcpy(this->mountPoint, mountPoint);
strcpy(this->user, user);
strcpy(this->password, password);
this->ntripClient = new WiFiClient;
this->state = NTRIPClientStates::closeConnection;
this->loopDelay = 20;
NTRIPClient::NTRIPClient(SFE_UBLOX_GNSS *gps, const char *host, uint16_t port, const char *mountPoint, const char *user, const char *password)
: gps{gps}, port{port}, host{host}, mountPoint{mountPoint}, user{user}, password{password}, ntripClient{new WiFiClient}, state{NTRIPClientStates::closeConnection}
{
Component::loopDelay = NTRIPClient::loopDelay;
}
NTRIPClient::~NTRIPClient() {
NTRIPClient::~NTRIPClient()
{
delete this->ntripClient;
}
void NTRIPClient::run() {
switch (this->state) {
case NTRIPClientStates::openConnection:
if (!this->activated) {
this->state = NTRIPClientStates::closeConnection;
break;
}
std::cout << "Connecting to the NTRIP caster..." << std::endl;
if (this->beginClient()) {
std::cout << "Connected to the NTRIP caster!" << std::endl;
this->state = NTRIPClientStates::pushData;
} else {
std::cout << "Failed!" << std::endl;
this->state = NTRIPClientStates::wait;
this->activated = false;
}
break;
case NTRIPClientStates::pushData:
if (!processConnection() || !this->activated)
this->state = NTRIPClientStates::closeConnection;
break;
case NTRIPClientStates::closeConnection:
std::cout << "Closing the connection to the NTRIP caster..." << std::endl;
this->closeConnection();
state = NTRIPClientStates::wait;
break;
case NTRIPClientStates::wait:
if (this->activated)
this->state = NTRIPClientStates::openConnection;
else
this->checkAutoReconnect();
break;
case NTRIPClientStates::notAvailable:
break;
default:
std::cout << "Wrong state in NTRIPClient.cpp..." << std::endl;
void NTRIPClient::run()
{
switch (this->state)
{
case NTRIPClientStates::openConnection:
if (!this->activated)
{
this->state = NTRIPClientStates::closeConnection;
break;
}
std::cout << "Connecting to the NTRIP caster..." << std::endl;
if (this->beginClient())
{
std::cout << "Connected to the NTRIP caster!" << std::endl;
this->state = NTRIPClientStates::pushData;
}
else
{
std::cout << "Failed!" << std::endl;
this->state = NTRIPClientStates::wait;
this->activated = false;
}
break;
case NTRIPClientStates::pushData:
if (!processConnection() || !this->activated)
{
this->state = NTRIPClientStates::closeConnection;
}
break;
case NTRIPClientStates::closeConnection:
std::cout << "Closing the connection to the NTRIP caster..." << std::endl;
this->closeConnection();
state = NTRIPClientStates::wait;
break;
case NTRIPClientStates::wait:
if (this->activated)
{
this->state = NTRIPClientStates::openConnection;
}
else
{
this->checkAutoReconnect();
}
break;
case NTRIPClientStates::notAvailable:
break;
default:
std::cout << "Wrong state in NTRIPClient.cpp..." << std::endl;
this->state = NTRIPClientStates::closeConnection;
break;
}
}
void NTRIPClient::runAsChild() {
void NTRIPClient::runAsChild()
{
this->pushGPGGA();
}
void NTRIPClient::gpsConfiguration() {
void NTRIPClient::gpsConfiguration()
{
this->gps->setSPIOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
this->gps->setPortInput(COM_PORT_SPI, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3);
// Set the differential mode - ambiguities are fixed whenever possible
@@ -90,21 +96,28 @@ void NTRIPClient::gpsConfiguration() {
this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
}
bool NTRIPClient::setActivated(bool state) {
// std::cout << "NTRIPClient::setActivated: b - " << b << std::endl;
bool NTRIPClient::setActivated(bool state)
{
if (state && this->state != NTRIPClientStates::notAvailable)
{
this->activated = true;
}
else if (state)
{
return false;
else {
}
else
{
this->activated = false;
this->autoReconnect = false;
}
return true;
}
void NTRIPClient::setAutoReconnect(bool state) {
if (state) {
void NTRIPClient::setAutoReconnect(bool state)
{
if (state)
{
this->reconnectAttemps = 0;
this->autoReconnect = true;
return;
@@ -113,61 +126,67 @@ void NTRIPClient::setAutoReconnect(bool state) {
this->autoReconnect = false;
}
bool NTRIPClient::beginClient() {
bool NTRIPClient::beginClient()
{
static constexpr uint16_t httpError = 401;
static constexpr uint16_t httpCheck = 200;
std::cout << "Opening socket to " << this->host << std::endl;
char serverRequest[this->bufferSize];
char credentials[this->bufferSize];
if (!this->ntripClient->connect(this->host, this->port)) {
if (!static_cast<bool>(this->ntripClient->connect(static_cast<const char *>(this->host), this->port)))
{
std::cout << "Connection to caster failed" << std::endl;
return false;
} else {
std::cout << "Connected to " << this->host << " : " << this->port << std::endl;
std::cout << "Requesting NTRIP Data from mount point " << this->mountPoint << std::endl;
// Generate the server request (GET)
snprintf(serverRequest,
this->bufferSize,
"GET /%s HTTP/1.0\r\nUser-Agent: NTRIP SparkFun u-blox Client v1.0\r\n",
this->mountPoint);
// Credentials
uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 2;
char* userCredentials = new char[userCredentialsLength];
snprintf(userCredentials, userCredentialsLength, "%s:%s", this->user, this->password);
std::cout << "Sending credentials: " << userCredentials << std::endl;
//Encode
base64 b;
String strEncodedCredentials = b.encode(userCredentials);
delete userCredentials;
char encodedCredentials[strEncodedCredentials.length() + 1];
strEncodedCredentials.toCharArray(encodedCredentials, sizeof(encodedCredentials));
snprintf(credentials, sizeof(credentials), "Authorization: Basic %s\r\n", encodedCredentials);
}
std::cout << "Connected to " << this->host << " : " << this->port << std::endl;
std::cout << "Requesting NTRIP Data from mount point " << this->mountPoint << std::endl;
// Generate the server request (GET)
snprintf(static_cast<char *>(serverRequest),
this->bufferSize,
static_cast<const char *>("GET /%s HTTP/1.0\r\nUser-Agent: NTRIP SparkFun u-blox Client v1.0\r\n"),
this->mountPoint);
// Credentials
const uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 2;
auto *userCredentials = new char[userCredentialsLength];
snprintf(static_cast<char *>(userCredentials), userCredentialsLength, static_cast<const char *>("%s:%s"), this->user, this->password);
std::cout << "Sending credentials: " << userCredentials << std::endl;
// Encode
const base64 base;
const String strEncodedCredentials = base64::encode(userCredentials);
delete userCredentials;
char encodedCredentials[strEncodedCredentials.length() + 1];
strEncodedCredentials.toCharArray(static_cast<char *>(encodedCredentials), sizeof(encodedCredentials));
snprintf(credentials, sizeof(credentials), static_cast<const char *>("Authorization: Basic %s\r\n"), static_cast<const char *>(encodedCredentials));
// Add the encoded credentials to the server request
strncat(serverRequest, credentials, this->bufferSize);
strncat(serverRequest, "\r\n", this->bufferSize);
strncat(static_cast<char *>(serverRequest), static_cast<const char *>(credentials), this->bufferSize);
strncat(static_cast<char *>(serverRequest), static_cast<const char *>("\r\n"), this->bufferSize);
std::cout << "serverRequest size: "
<< strlen(serverRequest)
<< " of "
<< this->bufferSize
<< " bytes available"
<< std::endl;
std::cout << static_cast<const char *>("serverRequest size: ")
<< strlen(serverRequest)
<< static_cast<const char *>(" of ")
<< this->bufferSize
<< static_cast<const char *>(" bytes available")
<< std::endl;
// Send the server request
std::cout << "Sending server request: " << serverRequest << std::endl;
this->ntripClient->write(serverRequest, strlen(serverRequest));
//Wait up to 5 seconds for response
uint32_t lastMillis = millis();
while (!ntripClient->available()) {
if (millis() - lastMillis > this->timeOut) {
// Wait up to 5 seconds for response
const uint32_t lastMillis = millis();
while (static_cast<bool>(!ntripClient->available()))
{
if (millis() - lastMillis > this->timeOut)
{
std::cout << "Caster timed out!" << std::endl;
this->ntripClient->stop();
return false;
@@ -175,35 +194,48 @@ bool NTRIPClient::beginClient() {
delay(10);
}
//Check reply
// Check reply
uint16_t httpStatusCode = 0;
char response[this->bufferSize];
uint16_t responseIndex = 0;
while (this->ntripClient->available()) {
while (static_cast<bool>(this->ntripClient->available()))
{
if (responseIndex == sizeof(response))
{
break;
}
response[responseIndex++] = ntripClient->read();
if (httpStatusCode == 0) {
if (strstr(response, "200") != nullptr)
httpStatusCode = 200;
if (strstr(response, "401") != nullptr)
httpStatusCode = 401;
if (httpStatusCode == 0)
{
if (strstr(response, static_cast<const char *>("200")) != nullptr)
{
httpStatusCode = httpCheck;
}
if (strstr(response, static_cast<const char *>("401")) != nullptr)
{
httpStatusCode = httpError;
}
}
}
response[responseIndex] = '\0';
// std::cout << "Caster response: " << response << std::endl;
if (httpStatusCode != 200) {
std::cout << "Failed to connect to " << this->host << " - HTTP Code: " << (int) httpStatusCode
if (httpStatusCode != httpCheck)
{
std::cout << "Failed to connect to " << this->host << " - HTTP Code: " << (int)httpStatusCode
<< " Length of Response: " << responseIndex << std::endl;
if (httpStatusCode == 0)
{
std::cout << "Response: " << response << std::endl;
else if (httpStatusCode == 401)
}
else if (httpStatusCode == httpError)
{
std::cout << "Statuscode 401 - Unauthorized" << std::endl;
}
return false;
}
@@ -212,35 +244,47 @@ bool NTRIPClient::beginClient() {
return true;
}
void NTRIPClient::closeConnection() {
if (this->ntripClient->connected())
void NTRIPClient::closeConnection()
{
if (static_cast<bool>(this->ntripClient->connected()))
{
this->ntripClient->stop();
}
this->activated = false;
std::cout << "NtripClient disconnected from: " << this->host << std::endl;
}
bool NTRIPClient::processConnection() {
if (this->ntripClient->connected()) {
bool NTRIPClient::processConnection()
{
if (static_cast<bool>(this->ntripClient->connected()))
{
uint8_t rtcmData[this->bufferSize * 8];
uint16_t rtcmCount = 0;
while (this->ntripClient->available()) {
while (static_cast<bool>(this->ntripClient->available()))
{
rtcmData[rtcmCount++] = ntripClient->read();
if (rtcmCount == sizeof(rtcmData))
{
break;
}
}
if (rtcmCount > 0) {
if (rtcmCount > 0)
{
this->lastReceivedRtcmTime = millis();
this->gps->pushRawData(rtcmData, rtcmCount);
this->gps->pushRawData(static_cast<uint8_t *>(rtcmData), rtcmCount);
// std::cout << "Pushed " << rtcmCount << " RTCM bytes to ZED." << std::endl;
}
} else {
}
else
{
std::cout << "Connection to " << this->host << " dropped!" << std::endl;
return false;
}
if (millis() - this->lastReceivedRtcmTime > this->timeOut) {
if (millis() - this->lastReceivedRtcmTime > this->timeOut)
{
std::cout << "RTCM timeout!" << std::endl;
return false;
}
@@ -248,15 +292,21 @@ bool NTRIPClient::processConnection() {
return true;
}
void NTRIPClient::checkAutoReconnect() {
void NTRIPClient::checkAutoReconnect()
{
if (!this->autoReconnect)
{
return;
}
if (millis() - this->lastReconnectTime < this->reconnectDelayTime)
{
return;
}
this->lastReconnectTime = millis();
if (this->reconnectAttemps >= this->maxReconnectAttemps) {
if (this->reconnectAttemps >= this->maxReconnectAttemps)
{
this->autoReconnect = false;
return;
}
@@ -265,26 +315,34 @@ void NTRIPClient::checkAutoReconnect() {
this->reconnectAttemps++;
}
void NTRIPClient::pushGPGGA() {
void NTRIPClient::pushGPGGA()
{
if (!this->transmitLocation && !this->activated)
{
return;
}
if (millis() - this->lastGPGGAPushTime < this->pushGPGGATime)
{
return;
}
this->lastGPGGAPushTime = millis();
if (!this->ntripClient->connected())
{
std::cout << "Failed to pushing GGA to server: " << std::endl;
NMEA_GGA_data_t *data = new NMEA_GGA_data_t;
uint8_t res = this->gps->getLatestNMEAGPGGA(data);
}
auto *data = new NMEA_GGA_data_t;
const uint8_t res = this->gps->getLatestNMEAGPGGA(data);
if (res == 2)
this->ntripClient->print((const char *)data);
{
this->ntripClient->print(reinterpret_cast<const char *>(data));
}
delete data;
}
bool NTRIPClient::isConnected() {
if (this->state == NTRIPClientStates::pushData)
return true;
return false;
bool NTRIPClient::isConnected()
{
return this->state == NTRIPClientStates::pushData;
}
lib/NtripClient/ntripClient.h
+90 -86
View File
@@ -4,9 +4,9 @@
* @brief Contains the class NTRIPClient
* @version 0.1
* @date 2023-02-13
*
*
* @copyright Copyright (c) 2023
*
*
*/
#ifndef NTRIP_CLIENT
@@ -22,109 +22,113 @@
#include "component.h"
/**
* @brief States for the state machine.
*
* @brief States for the state machine.
*
*/
enum NTRIPClientStates {
openConnection,
pushData,
closeConnection,
wait,
notAvailable
enum NTRIPClientStates
{
openConnection,
pushData,
closeConnection,
wait,
notAvailable
};
/**
* @brief Ntrip Client
*
*
* This class can connect to a ntrip server to pull correction
* data and push it to a given gnss module. This module have to be compatible
* with the SparkFun u-blox GNSS Arduino Library.
*/
class NTRIPClient : public Component {
public:
/**
* @brief Construct a new NTRIPClient object
*
* @param gps The Gnss module
* @param host
* @param port
* @param mountPoint
* @param user
* @param password
*/
NTRIPClient(SFE_UBLOX_GNSS* gps, const char* host, uint16_t port, const char* mountPoint, const char* user, const char* password);
~NTRIPClient();
class NTRIPClient : public Component
{
public:
/**
* @brief Construct a new NTRIPClient object
*
* @param gps The Gnss module
* @param host
* @param port
* @param mountPoint
* @param user
* @param password
*/
NTRIPClient(SFE_UBLOX_GNSS *gps, const char *host, uint16_t port, const char *mountPoint, const char *user, const char *password);
~NTRIPClient();
/**
* @brief Configure the Gnss module to accept correction data
*
*/
void gpsConfiguration();
/**
* @brief Configure the Gnss module to accept correction data
*
*/
void gpsConfiguration();
/**
* @brief Activate or deactivate the location transmission
*
* Some server need the position of the Gnss module to send the
* right correction data.
*
* @param b
*/
void setTransmitLocation(bool b) { this->transmitLocation = b; }
/**
* @brief Activate or deactivate the location transmission
*
* Some server need the position of the Gnss module to send the
* right correction data.
*
* @param b
*/
void setTransmitLocation(bool b) { this->transmitLocation = b; }
/**
* @brief Activate or deactivate the connection to the server.
*
* @param state
* @return true success
* @return false failure
*/
bool setActivated(bool state);
void setAutoReconnect(bool state);
/**
* @brief Activate or deactivate the connection to the server.
*
* @param state
* @return true success
* @return false failure
*/
bool setActivated(bool state);
void setAutoReconnect(bool state);
bool isConnected();
bool isConnected();
/**
* @brief Get the Client State object
*
* Returns the state of the State machine
*
* @return NTRIPClientStates
*/
NTRIPClientStates getClientState() { return this->state; }
/**
* @brief Get the Client State object
*
* Returns the state of the State machine
*
* @return NTRIPClientStates
*/
NTRIPClientStates getClientState() { return this->state; }
private:
void run() override;
void runAsChild() override;
void pushGPGGA();
bool beginClient();
void closeConnection();
bool processConnection();
void checkAutoReconnect();
private:
void run() override;
void runAsChild() override;
void pushGPGGA();
bool beginClient();
void closeConnection();
bool processConnection();
void checkAutoReconnect();
SFE_UBLOX_GNSS* gps;
WiFiClient* ntripClient;
NTRIPClientStates state = NTRIPClientStates::notAvailable;
SFE_UBLOX_GNSS *gps;
WiFiClient *ntripClient;
NTRIPClientStates state = NTRIPClientStates::notAvailable;
bool transmitLocation = false;
bool activated = true;
bool autoReconnect = false;
bool transmitLocation = false;
bool activated = true;
bool autoReconnect = false;
uint8_t reconnectAttemps = 0;
uint16_t port;
uint32_t lastReceivedRtcmTime = 0;
// uint32_t lastNtripConnectTime = 0; // can deleted?
uint32_t lastGPGGAPushTime = 0;
uint32_t lastReconnectTime = 0;
uint8_t reconnectAttemps = 0;
uint16_t port;
uint32_t lastReceivedRtcmTime = 0;
// uint32_t lastNtripConnectTime = 0; // can deleted?
uint32_t lastGPGGAPushTime = 0;
uint32_t lastReconnectTime = 0;
char host[128];
char mountPoint[128];
char user[128];
char password[128];
const uint8_t maxReconnectAttemps = 10;
const uint16_t reconnectDelayTime = 1000;
const uint16_t timeOut = 10000;
const uint16_t bufferSize = 512;
const uint16_t pushGPGGATime = 10000;
const char *host;
const char *mountPoint;
const char *user;
const char *password;
const uint8_t maxReconnectAttemps = 10;
const uint16_t reconnectDelayTime = 1000;
const uint16_t timeOut = 10000;
const uint16_t bufferSize = 512;
const uint16_t pushGPGGATime = 10000;
static constexpr uint8_t loopDelay = 20;
};
#endif
lib/Point/point.cpp
+57 -43
View File
@@ -1,104 +1,118 @@
/**
* @file point.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief
* @version 0.1
* @date 2023-09-03
*
*
* @copyright Copyright (c) 2023
*
*
*/
#include "point.h"
Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates.lat = lat;
this->coordinates.lon = lon;
Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime)
: coordinates{lat, lon}
{
this->init(horizontalAccuracy, creationTime);
}
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;
Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime)
: coordinates{lat / 10000000.0, lon / 10000000.0}
{
this->init(horizontalAccuracy, creationTime);
}
Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates = coords;
Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime)
: coordinates{coords}
{
this->init(horizontalAccuracy, creationTime);
}
Point::Point(Coordinates coords, bool imported) {
Point::Point(Coordinates coords, bool imported)
{
this->coordinates = coords;
if (imported)
{
this->init(UINT32_MAX, 0);
}
else
{
this->init(0, 0);
}
}
Point::Point() {
Point::Point()
{
this->coordinates.lat = 0;
this->coordinates.lon = 0;
this->coordinates.lon = 0;
this->init(0, 0);
}
bool Point::operator==(const Point& rhs) const {
bool Point::operator==(const Point &rhs) const
{
return this->coordinates == rhs.getCoordinates();
}
// distance = sqrt(dx * dx + dy * dy)
// mit distance: Entfernung in km
// dx = 111.3 * cos(lat) * (lon1 - lon2)
// lat = (lat1 + lat2) / 2 * 0.01745
// dy = 111.3 * (lat1 - lat2)
// lat1, lat2, lon1, lon2: Breite, Länge in Grad
double Point::distanceTo(const Coordinates& point) const {
Coordinates begin = this->coordinates;
Coordinates end = point;
double Point::distanceTo(const Coordinates &point) const
{
const Coordinates begin = this->coordinates;
const Coordinates end = point;
double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
const double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
const double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
const double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
return sqrt(dx * dx + dy * dy);
}
double Point::distanceTo(const Point &point) const {
double Point::distanceTo(const Point &point) const
{
return this->distanceTo(point.getCoordinates());
}
int16_t Point::courseTo(const Coordinates& point) const {
Coordinates begin = this->coordinates;
Coordinates end = point;
int16_t Point::courseTo(const Coordinates &point) const
{
const Coordinates begin = this->coordinates;
const Coordinates end = point;
double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) );
double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
const double phi = log(tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4));
const double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
int16_t res = static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
// if (res < 0)
// res += 360;
return res;
return static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
}
int16_t Point::courseTo(const Point &point) const {
int16_t Point::courseTo(const Point &point) const
{
return this->courseTo(point.getCoordinates());
}
void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) {
void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime)
{
this->creationTime = creationTime;
if (horizontalAccuracy == UINT32_MAX)
{
this->accuracy = Accuracy::imported;
}
else if (horizontalAccuracy > 9999)
{
this->accuracy = Accuracy::fourDigOfCM;
}
else if (horizontalAccuracy > 999)
{
this->accuracy = Accuracy::threeDigOfCM;
}
else if (horizontalAccuracy > 99)
{
this->accuracy = Accuracy::twoDigOfCM;
}
else if (horizontalAccuracy > 1)
{
this->accuracy = Accuracy::oneDigOfCM;
else
this->accuracy = Accuracy::none;
}
else
{
this->accuracy = Accuracy::none;
}
}
lib/Point/point.h
+108 -105
View File
@@ -1,139 +1,142 @@
/**
* @file point.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief
* @version 0.1
* @date 2023-09-03
*
*
* @copyright Copyright (c) 2023
*
*
*/
#ifndef POINT_H
#define POINT_H
#include <cmath>
#include <cmath>
#define ROUTE_DEGREE_TO_RADIANT 0.01745
#define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300
/**
* @brief A to handle points on the earth
*
*
* The points inherits latidue and longitude as doubles
*
*
*/
class Point{
public:
/**
* @brief Hold the data longitude and latitude
*
*/
struct Coordinates {
double lon;
double lat;
class Point
{
public:
/**
* @brief Hold the data longitude and latitude
*
*/
struct Coordinates
{
double lon;
double lat;
bool operator==(const Coordinates rhs) const {
return ( this->lon == rhs.lon ) && ( this->lon == rhs.lon );
}
};
bool operator==(const Coordinates rhs) const
{
return (this->lon == rhs.lon) && (this->lon == rhs.lon);
}
};
/**
* @brief The Accuracy is set by the constructor
*
*/
enum Accuracy {
none,
fourDigOfCM,
threeDigOfCM,
twoDigOfCM,
oneDigOfCM,
imported
};
/**
* @brief The Accuracy is set by the constructor
*
*/
enum Accuracy
{
none,
fourDigOfCM,
threeDigOfCM,
twoDigOfCM,
oneDigOfCM,
imported
};
/**
* @brief Construct a new Point object
*
* @param lat latitude
* @param lon longitude
* @param horizontalAccuracy mm
* @param coords Coordinates
* @param imported if true than highest accuracy
*/
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();
/**
* @brief Construct a new Point object
*
* @param lat latitude
* @param lon longitude
* @param horizontalAccuracy mm
* @param coords Coordinates
* @param imported if true than highest accuracy
*/
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();
/**
* @brief Checks if to points are equal.
*
* @param rhs
* @return true
* @return false
*/
bool operator==(const Point &rhs) const;
/**
* @brief Checks if to points are equal.
*
* @param rhs
* @return true
* @return false
*/
bool operator==(const Point& rhs) const;
/**
* @brief Checks if the point is initalized.
*
* @return true
* @return false
*/
bool isInit() const { return this->coordinates.lat + this->coordinates.lon; }
/**
* @brief Checks if the point is initalized.
*
* @return true
* @return false
*/
bool isInit() const { return this->coordinates.lat + this->coordinates.lon; }
/**
* @brief Checks if the point is valid.
*
* If the accuracy is higher than zero, true will be returned.
*
* @return true
* @return false
*/
bool isValid() const { return (this->accuracy > 0) ? true : false; }
/**
* @brief Checks if the point is valid.
*
* If the accuracy is higher than zero, true will be returned.
*
* @return true
* @return false
*/
bool isValid() const { return (this->accuracy > 0) ? true : false; }
/**
* @brief Calculates the distance between to points.
*
* @param point
* @return double meter
*/
double distanceTo(const Coordinates &point) const;
double distanceTo(const Point &point) const;
/**
* @brief Calculates the distance between to points.
*
* @param point
* @return double meter
*/
double distanceTo(const Coordinates& point) const;
double distanceTo(const Point& point) const;
/**
* @brief Calculates the course to an other point.
*
* @param point
* @return int16_t degree
*/
int16_t courseTo(const Coordinates &point) const;
int16_t courseTo(const Point &point) const;
/**
* @brief Calculates the course to an other point.
*
* @param point
* @return int16_t degree
*/
int16_t courseTo(const Coordinates& point) const;
int16_t courseTo(const Point& point) const;
uint32_t getCreationTime() const { return this->creationTime; }
double getLongitude() const { return this->coordinates.lon; }
double getLatitude() const { return this->coordinates.lat; }
Coordinates getCoordinates() const { return this->coordinates; }
uint32_t getCreationTime() const { return this->creationTime; }
double getLongitude() const { return this->coordinates.lon; }
double getLatitude() const { return this->coordinates.lat; }
Coordinates getCoordinates() const { return this->coordinates; }
/**
* @brief Get the Accuracy object
*
* The higher the value, the greater the accuracy.
* You can check it by Accuracy.
*
* @return Accuracy
*/
Accuracy getAccuracy() const { return this->accuracy; }
/**
* @brief Get the Accuracy object
*
* The higher the value, the greater the accuracy.
* You can check it by Accuracy.
*
* @return Accuracy
*/
Accuracy getAccuracy() const { return this->accuracy; }
private:
void init(uint32_t horizontalAccuracy, uint32_t creationTime);
private:
void init(uint32_t horizontalAccuracy, uint32_t creationTime);
Accuracy accuracy = Accuracy::none;
Coordinates coordinates{0, 0};
Accuracy accuracy = Accuracy::none;
Coordinates coordinates;
uint32_t creationTime = 0;
uint32_t creationTime = 0;
};
#endif //POINT_H
#endif // POINT_H
lib/Route/route.cpp
+40 -28
View File
@@ -4,29 +4,29 @@
* @brief Implements the class Route and Point
* @version 0.1
* @date 2022-01-31
*
*
* @copyright Copyright (c) 2022
*
*
*/
#include "route.h"
Route::Route() {
}
void Route::addPointToRoute(Point point) {
void Route::addPointToRoute(Point point)
{
this->points.push_back(point);
}
void Route::clear() {
void Route::clear()
{
this->points.clear();
this->currentPoint = 0;
this->started = false;
}
Point Route::startRoute() {
if (this->points.size() < 1) {
Point Route::startRoute()
{
if (this->points.empty())
{
this->started = false;
return Point();
}
@@ -37,8 +37,10 @@ Point Route::startRoute() {
return *this->it;
}
Point Route::endRoute() {
if (this->points.size() < 1) {
Point Route::endRoute()
{
if (this->points.empty())
{
this->started = false;
return Point();
}
@@ -51,37 +53,47 @@ Point Route::endRoute() {
return *this->it;
}
Point Route::getNextPoint() {
Point Route::getNextPoint()
{
Point point;
if (!this->started)
return Point();
{
return point;
}
if (this->it != --this->points.end()) {
if (this->it != --this->points.end())
{
this->it++;
this->currentPoint++;
return *this->it;
// } else if (this->it == this->points.end() && this->currentPoint != this->points.size()) {
// this->currentPoint++;
// return *this->it;
// } else if (this->it == this->points.end() && this->currentPoint != this->points.size()) {
// this->currentPoint++;
// return *this->it;
}
return Point();
return point;
}
Point Route::getPreviousPoint() {
Point Route::getPreviousPoint()
{
Point point;
if (!this->started)
return Point();
if (this->it != this->points.begin()) {
{
return point;
}
if (this->it != this->points.begin())
{
this->it--;
this->currentPoint--;
return *this->it;
} else {
return Point();
}
return point;
}
RouteInfo Route::getRouteInfo() {
RouteInfo info;
RouteInfo Route::getRouteInfo()
{
RouteInfo info{0, 0};
info.totalPoints = this->points.size();
info.currentPoint = this->currentPoint;
return info;
lib/Route/route.h
-5
View File
@@ -40,11 +40,6 @@ struct RouteInfo{
*/
class Route {
public:
/**
* @brief Construct a new Route object.
*/
Route();
/**
* @brief Adds a point to the list.
*
lib/Sensors/sensorData.cpp
+138 -71
View File
@@ -1,30 +1,32 @@
/**
* @file senors.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief
* @version 0.1
* @date 2023-09-02
*
*
* @copyright Copyright (c) 2023
*
*
*/
#include "sensorData.h"
bool SensorData::outputStatusPrintPVTdata = false;
uint32_t SensorData::ubxUpdateTimeStatic = 0;
UBX_NAV_PVT_data_t* SensorData::ubxDataStatic = nullptr;
UBX_NAV_PVT_data_t *SensorData::ubxDataStatic = nullptr;
SensorData::SensorData() {
this->loopDelay = 50;
SensorData::SensorData()
{
Component::loopDelay = SensorData::loopDelay;
}
SensorData::~SensorData() {
if (this->ntripClient)
delete this->ntripClient;
SensorData::~SensorData()
{
delete this->ntripClient;
}
void SensorData::enableNtrip(String host, uint16_t port, String mountPoint, String user, String password) {
void SensorData::enableNtrip(String host, uint16_t port, String mountPoint, String user, String password)
{
this->ntripClient = new NTRIPClient(this->gnss, host.c_str(), port, mountPoint.c_str(), user.c_str(), password.c_str());
this->ntripClient->gpsConfiguration();
this->ntripClient->loop();
@@ -33,120 +35,176 @@ void SensorData::enableNtrip(String host, uint16_t port, String mountPoint, Stri
this->addChildComponent(this->ntripClient);
}
void SensorData::enableGnss(SPIClass* spiPort, uint8_t csPin) {
void SensorData::enableGnss(SPIClass *spiPort, uint8_t csPin)
{
this->gnss = new SFE_UBLOX_GNSS();
if (this->gnss->begin(*spiPort, csPin, 4000000) == false) {
if (this->gnss->begin(*spiPort, csPin, 4000000) == false)
{
std::cout << "u-blox GNSS not detected on SPI bus. Please check wiring. Freezing." << std::endl;
while (1);
while (true)
{
}
}
this->initGnss();
}
void SensorData::enableGnss() {
void SensorData::enableGnss()
{
this->gnss = new SFE_UBLOX_GNSS();
if (this->gnss->begin() == false) {
if (this->gnss->begin() == false)
{
std::cout << "u-blox GNSS not detected at default I2C address. Please check wiring. Freezing." << std::endl;
while (1);
while (true)
{
}
}
this->initGnss();
}
void SensorData::enableRealCompass() {
void SensorData::enableRealCompass()
{
static constexpr byte address = 0x0d;
this->realCompass = new QMC5883LCompass();
// Init Compass
Wire.beginTransmission(0x0d);
Wire.beginTransmission(address);
// TODO: describe Bytes !!!
Wire.write(0x0b);
Wire.write(0x01);
Wire.endTransmission();
this->realCompass->setMode(0x01,0x0C,0x10,0X00);
this->realCompass->setMode(0x01, 0x0C, 0x10, 0X00);
CalibrateCompass caliCompass(this->realCompass);
caliCompass.loadData();
caliCompass.useData();
}
void SensorData::enableCalcCompass() {
void SensorData::enableCalcCompass()
{
// TODO: !!! implementieren
}
void SensorData::enableGyroskop() {
void SensorData::enableGyroskop()
{
this->gyroskop = new MPU6050();
this->gyroskop->initialize();
if (!this->gyroskop->testConnection()) {
if (!this->gyroskop->testConnection())
{
std::cout << "SensorData::enableGyroskop: Gyroskop is not conntected. Freeze!" << std::endl;
while (true);
while (true)
{
}
}
uint8_t deviceStatus = this->gyroskop->dmpInitialize();
const uint8_t deviceStatus = this->gyroskop->dmpInitialize();
// TODO: !!! MagicNumer 6x
this->gyroskop->setXGyroOffset(220);
this->gyroskop->setYGyroOffset(76);
this->gyroskop->setZGyroOffset(-85);
this->gyroskop->setZAccelOffset(1788);
if (deviceStatus == 0) {
if (deviceStatus == 0)
{
this->gyroskop->CalibrateAccel(6);
this->gyroskop->CalibrateGyro(6);
this->gyroskop->PrintActiveOffsets();
this->gyroskop->setDMPEnabled(true);
} else {
}
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);
std::cout << "SensorData::enableGyroskop: DMP Initialization failed (code" << static_cast<int>(deviceStatus) << "). Freeze!" << std::endl;
while (true)
{
}
}
}
CalcAzimuth::State SensorData::getCalcAzimuthState() const {
if (this->calcCompass)
CalcAzimuth::State SensorData::getCalcAzimuthState() const
{
if (static_cast<bool>(this->calcCompass))
{
return this->calcCompass->getState();
}
return CalcAzimuth::State::Invalid;
}
NTRIPClientStates SensorData::getNtripState() const {
if (this->ntripClient)
NTRIPClientStates SensorData::getNtripState() const
{
if (static_cast<bool>(this->ntripClient))
{
return this->ntripClient->getClientState();
}
return NTRIPClientStates::notAvailable;
}
void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct)
{
static constexpr uint8_t stringSize = 32;
if (!SensorData::outputStatusPrintPVTdata)
{
return;
}
double latitude = (double) ubxDataStruct->lat / 10000000.0;
double longitude = (double) ubxDataStruct->lon / 10000000.0;
double altitude = (double) ubxDataStruct->hMSL / 1000.0;
const double latitude = ubxDataStruct->lat / 10000000.0;
const double longitude = ubxDataStruct->lon / 10000000.0;
const double altitude = ubxDataStruct->hMSL / 1000.0;
uint8_t fixType = ubxDataStruct->fixType;
char fixTypeString[32];
const uint8_t fixType = ubxDataStruct->fixType;
char fixTypeString[stringSize];
if (fixType == 0)
strcpy(fixTypeString, "None");
{
strcpy(fixTypeString, static_cast<const char *>("None"));
}
else if (fixType == 1)
strcpy(fixTypeString, "Dead Reckoning");
{
strcpy(fixTypeString, static_cast<const char *>("Dead Reckoning"));
}
else if (fixType == 2)
strcpy(fixTypeString, "2D");
{
strcpy(fixTypeString, static_cast<const char *>("2D"));
}
else if (fixType == 3)
strcpy(fixTypeString, "3D");
{
strcpy(fixTypeString, static_cast<const char *>("3D"));
}
else if (fixType == 3)
strcpy(fixTypeString, "GNSS + Dead Reckoning");
{
strcpy(fixTypeString, static_cast<const char *>("GNSS + Dead Reckoning"));
}
else if (fixType == 5)
strcpy(fixTypeString, "Time Only");
{
strcpy(fixTypeString, static_cast<const char *>("Time Only"));
}
else
strcpy(fixTypeString, "UNKNOWN");
{
strcpy(fixTypeString, static_cast<const char *>("UNKNOWN"));
}
uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln;
char carrSolnString[16];
const uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln;
char carrSolnString[stringSize];
if (carrSoln == 0)
strcpy(carrSolnString, "None");
{
strcpy(carrSolnString, static_cast<const char *>("None"));
}
else if (carrSoln == 1)
strcpy(carrSolnString, "Floating");
{
strcpy(carrSolnString, static_cast<const char *>("Floating"));
}
else if (carrSoln == 2)
strcpy(carrSolnString, "Fixed");
{
strcpy(carrSolnString, static_cast<const char *>("Fixed"));
}
else
strcpy(carrSolnString, "UNKNOWN");
{
strcpy(carrSolnString, static_cast<const char *>("UNKNOWN"));
}
uint32_t hAcc = ubxDataStruct->hAcc;
const uint32_t hAcc = ubxDataStruct->hAcc;
std::cout << "Lat: " << latitude
<< " Lng: " << longitude
@@ -157,45 +215,53 @@ void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
<< " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl;
}
void SensorData::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
void SensorData::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct)
{
SensorData::printPVTdata(ubxDataStruct);
SensorData::ubxDataStatic = ubxDataStruct;
SensorData::ubxUpdateTimeStatic = millis();
}
void SensorData::setOutputStatusPrintPVTdata(bool status) {
void SensorData::setOutputStatusPrintPVTdata(bool status)
{
SensorData::outputStatusPrintPVTdata = status;
}
void SensorData::run() {
if (this->realCompass) {
void SensorData::run()
{
if (static_cast<bool>(this->realCompass))
{
this->realCompass->read();
this->realAzimuth = this->realCompass->getAzimuth();
}
if (this->gyroskop
&& this->gyroskop->dmpGetCurrentFIFOPacket(this->gyroBuffer))
if (static_cast<bool>(this->gyroskop) && this->gyroskop->dmpGetCurrentFIFOPacket(static_cast<uint8_t *>(this->gyroBuffer)))
{
this->gyroskop->dmpGetQuaternion(&this->quaternion, this->gyroBuffer);
this->gyroskop->dmpGetQuaternion(&this->quaternion, static_cast<uint8_t *>(this->gyroBuffer));
this->gyroskop->dmpGetGravity(&this->gravity, &this->quaternion);
this->gyroskop->dmpGetYawPitchRoll(this->yawPitchRoll, &this->quaternion, &this->gravity);
this->gyroskop->dmpGetYawPitchRoll(static_cast<float *>(this->yawPitchRoll), &this->quaternion, &this->gravity);
}
}
void SensorData::runAsChild() {
if (this->gnss) {
void SensorData::runAsChild()
{
if (static_cast<bool>(this->gnss))
{
this->gnss->checkUblox();
this->gnss->checkCallbacks();
if (SensorData::ubxUpdateTimeStatic != this->lastUbxUpdate)
{
this->updateUbxData();
}
}
}
}
void SensorData::initGnss() {
uint8_t versionHigh = this->gnss->getProtocolVersionHigh();
uint8_t versionLow = this->gnss->getProtocolVersionLow();
std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl;
void SensorData::initGnss()
{
const uint8_t versionHigh = this->gnss->getProtocolVersionHigh();
const uint8_t versionLow = this->gnss->getProtocolVersionLow();
std::cout << "u-blox protocol version: " << static_cast<int>(versionHigh) << "." << static_cast<int>(versionLow) << std::endl;
this->gnss->setSPIOutput(COM_TYPE_UBX);
this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
@@ -206,11 +272,12 @@ void SensorData::initGnss() {
this->gnss->setAutoPVT(true);
}
void SensorData::updateUbxData() {
void SensorData::updateUbxData()
{
this->gnssData = SensorData::ubxDataStatic;
this->lastUbxUpdate = SensorData::ubxUpdateTimeStatic;
Point::Coordinates coords;
Point::Coordinates coords{0, 0};
coords.lat = this->gnssData->lat / 10000000.0;
coords.lon = this->gnssData->lon / 10000000.0;
lib/Sensors/sensorData.h
+71 -69
View File
@@ -1,12 +1,12 @@
/**
* @file sensorData.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief
* @version 0.1
* @date 2023-09-02
*
*
* @copyright Copyright (c) 2023
*
*
*/
#ifndef SENSOR_DATA_H
@@ -28,84 +28,86 @@
#include "point.h"
class Sensors;
class SensorData : public Component {
public:
SensorData();
~SensorData();
void enableNtrip(String host, uint16_t port, String mountPoint, String user, String password);
void enableGnss(SPIClass* spiPort, uint8_t csPin);
void enableGnss();
void enableRealCompass();
void enableCalcCompass();
void enableGyroskop();
class SensorData : public Component
{
public:
SensorData();
~SensorData();
// Interface Const kram
int16_t getRealAzimuth() const { return this->realAzimuth; }
int16_t getCalcAzimuth() const { return this->calcAzimuth; }
CalcAzimuth::State getCalcAzimuthState() const;
void enableNtrip(String host, uint16_t port, String mountPoint, String user, String password);
void enableGnss(SPIClass *spiPort, uint8_t csPin);
void enableGnss();
void enableRealCompass();
void enableCalcCompass();
void enableGyroskop();
Point getCurrentPos() const { return this->currentPosition; }
const UBX_NAV_PVT_data_t* getGnssData() const { return this->gnssData; };
NTRIPClientStates getNtripState() const;
const float* getGyroData() const { return this->yawPitchRoll; }
// Interface Const kram
int16_t getRealAzimuth() const { return this->realAzimuth; }
int16_t getCalcAzimuth() const { return this->calcAzimuth; }
CalcAzimuth::State getCalcAzimuthState() const;
CalcAzimuth* getCalcCompass() const { return this->calcCompass; }
QMC5883LCompass* getRealCompass() const { return this->realCompass; }
NTRIPClient* getNtripClient() const { return this->ntripClient; }
MPU6050* getGyroskop() const { return this->gyroskop; }
Point getCurrentPos() const { return this->currentPosition; }
const UBX_NAV_PVT_data_t *getGnssData() const { return this->gnssData; };
NTRIPClientStates getNtripState() const;
const float *getGyroData() const { return this->yawPitchRoll; }
// static
/**
* @brief Set the output status for PVTdata.
*
* If this is true, a lot of information from the gnss module will be printed in
* the interval of navigation frequency.
*
* @param status
*/
static void setOutputStatusPrintPVTdata(bool status);
CalcAzimuth *getCalcCompass() const { return this->calcCompass; }
QMC5883LCompass *getRealCompass() const { return this->realCompass; }
NTRIPClient *getNtripClient() const { return this->ntripClient; }
MPU6050 *getGyroskop() const { return this->gyroskop; }
private:
void run() override;
void runAsChild() override;
void initGnss();
void updateUbxData();
// static
/**
* @brief Set the output status for PVTdata.
*
* If this is true, a lot of information from the gnss module will be printed in
* the interval of navigation frequency.
*
* @param status
*/
static void setOutputStatusPrintPVTdata(bool status);
private:
void run() override;
void runAsChild() override;
void initGnss();
void updateUbxData();
QMC5883LCompass* realCompass = nullptr;
CalcAzimuth* calcCompass = nullptr;
SFE_UBLOX_GNSS* gnss = nullptr;
NTRIPClient* ntripClient = nullptr;
MPU6050* gyroskop = nullptr;
QMC5883LCompass *realCompass = nullptr;
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;
UBX_NAV_PVT_data_t *gnssData = nullptr;
Point currentPosition;
Quaternion quaternion;
VectorFloat gravity;
char* host;
char* mountPoint;
char* user;
char* password;
bool isNtripInit = false;
char *host = nullptr;
char *mountPoint = nullptr;
char *user = nullptr;
char *password = nullptr;
uint8_t gyroBuffer[64];
uint16_t port;
int16_t realAzimuth = INT16_MAX;
int16_t calcAzimuth = INT16_MAX;
uint32_t lastUbxUpdate = 0;
bool isNtripInit = false;
float yawPitchRoll[3] {0, 0, 0};
uint8_t gyroBuffer[64];
uint16_t port = 0;
int16_t realAzimuth = INT16_MAX;
int16_t calcAzimuth = INT16_MAX;
uint32_t lastUbxUpdate = 0;
// static
static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
float yawPitchRoll[3]{0, 0, 0};
static UBX_NAV_PVT_data_t* ubxDataStatic;
static uint32_t ubxUpdateTimeStatic;
static bool outputStatusPrintPVTdata;
// 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 constexpr uint8_t loopDelay = 50;
};
#endif //SENSOR_DATA_H
#endif // SENSOR_DATA_H
lib/Speedometer/speedometer.cpp
+76 -52
View File
@@ -5,18 +5,16 @@
* @see speedometer.h
* @version 0.1
* @date 2021-12-13
*
*
* @copyright Copyright (c) 2021
*
*
*/
#include "speedometer.h"
Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) {
this->diameter = diameter;
this->steps = steps;
this->pulseCounter = new Counter(pin);
this->pulseCounter->setFilterValue(1023); // ignore pulses less than 1000 x 2.5ns
Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps)
: pulseCounter{new Counter(pin)}, diameter{diameter}, steps{steps}, buf{}
{
this->pulseCounter->setFilterValue(Speedometer::maxFilterValue); // ignore pulses less than 1000 x 2.5ns
this->pulseCounter->clear();
this->pulseCounter->resume();
@@ -26,105 +24,131 @@ Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) {
this->clearAvgBuf();
}
Speedometer::~Speedometer() {
Speedometer::~Speedometer()
{
delete this->pulseCounter;
}
void Speedometer::run() {
void Speedometer::run()
{
static constexpr float minimalSpeed = 0.1;
if (this->calibrationRunning)
{
return;
}
uint32_t time = millis();
const uint32_t time = millis();
uint16_t elapsedTime = time - this->lastMillisCalc;
const uint16_t elapsedTime = time - this->lastMillisCalc;
this->lastMillisCalc = time;
int16_t pulse = this->pulseCounter->getValue();
const double pulse = this->pulseCounter->getValue();
this->pulseCounter->clear();
this->pulseCounter->resume();
double n = (double)pulse / this->steps; // Wheel revolutions in absolute time
double u = n / ((double)elapsedTime / 1000); // Wheel revolutions per second
double ms = u * (diameter * PI); // Speed in m/s
double rad = u * 2 * PI;
const double wheelRevolutionsAbsolute = pulse / this->steps;
const double wheelRevolutionsRelativ = wheelRevolutionsAbsolute / (elapsedTime / 1000.0);
if (speed < 0.1) {
speed = 0;
rad = 0;
double meterPerSecond = wheelRevolutionsRelativ * (diameter * PI);
double radPerSecond = wheelRevolutionsRelativ * 2 * PI;
if (meterPerSecond < minimalSpeed)
{
meterPerSecond = 0;
radPerSecond = 0;
}
switch (this->currentDirection) {
case Direction::Forward :
this->speed = ms;
this->rad = rad;
break;
case Direction::Backward :
this->speed = -ms;
this->rad = -rad;
break;
switch (this->currentDirection)
{
case Direction::Forward:
this->speed = meterPerSecond;
this->rad = radPerSecond;
break;
case Direction::None :
this->speed = 0;
this->rad = 0;
break;
case Direction::Backward:
this->speed = -meterPerSecond;
this->rad = -radPerSecond;
break;
case Direction::None:
this->speed = 0;
this->rad = 0;
break;
}
this->addValToBuf(static_cast<int16_t>(this->speed * Speedometer::conversionFactor));
}
void Speedometer::setDirection(Direction dir) {
void Speedometer::setDirection(Direction dir)
{
if (this->currentDirection == dir)
{
return;
}
this->currentDirection = dir;
this->clearAvgBuf();
}
void Speedometer::setEncFilter(uint16_t val) {
if (val > 1023)
val = 1023;
void Speedometer::setEncFilter(uint16_t val)
{
if (val > Speedometer::maxFilterValue)
{
val = Speedometer::maxFilterValue;
}
this->pulseCounter->setFilterValue(val);
}
double Speedometer::getAvgSpeed() const {
int16_t avg = this->calcAverage();
return (float)avg / Speedometer::conversionFactor;
double Speedometer::getAvgSpeed() const
{
const double avg = this->calcAverage();
return avg / Speedometer::conversionFactor;
}
void Speedometer::calibrationMeasurementStart() {
void Speedometer::calibrationMeasurementStart()
{
std::cout << "Start" << std::endl;
this->calibrationRunning = true;
this->pulseCounter->clear();
this->pulseCounter->resume();
}
uint16_t Speedometer::calibrationMeasurementStop() {
uint16_t Speedometer::calibrationMeasurementStop()
{
std::cout << "Ende" << std::endl;
this->calibrationRunning = false;
uint16_t res = abs(this->pulseCounter->getValue());
const uint16_t res = abs(this->pulseCounter->getValue());
this->pulseCounter->clear();
this->pulseCounter->resume();
std::cout << "Result: " << res << std::endl;
return res;
}
void Speedometer::clearAvgBuf() {
for (uint8_t i = 0; i < bufSize; i++)
void Speedometer::clearAvgBuf()
{
for (uint8_t i = 0; i < bufSize; i++)
{
this->buf[i] = 0;
}
}
void Speedometer::addValToBuf(int16_t val) {
void Speedometer::addValToBuf(int16_t val)
{
this->buf[this->bufPos] = val;
this->bufPos++;
if (bufPos == bufSize)
bufPos = 0;
{
bufPos = 0;
}
}
int16_t Speedometer::calcAverage() const {
int16_t Speedometer::calcAverage() const
{
int16_t sum = 0;
for (int i = 0; i < this->bufSize; i++)
for (int i = 0; i < Speedometer::bufSize; i++)
{
sum += this->buf[i];
return sum / this->bufSize;
}
return sum / Speedometer::bufSize;
}
lib/Speedometer/speedometer.h
+93 -92
View File
@@ -4,9 +4,9 @@
* @brief A implementation to measure wheel speeds with an encoder.
* @version 0.1
* @date 2021-12-09
*
*
* @copyright Copyright (c) 2021
*
*
*/
#ifndef SPEEDOMETER_H
@@ -21,114 +21,115 @@
/**
* @brief A class which use a encoder to calc the speed
*
*
* This class use ESP32 pulse counter hardware peripheral.
* The calculated speed is the average of an amount of last measurements.
*
*
*/
class Speedometer : public Component {
public:
/**
* @brief Enum to control the direction.
*
* If the Direction is Forward, the internal counter counts up and a positiv speed will be returned.
* If the Direction is Backward, the internal counter counts down and a negativ speed will be returned.
* If the Direction is None, no measurement will be taken.
*/
enum Direction {
None,
Forward,
Backward
};
class Speedometer : public Component
{
public:
/**
* @brief Enum to control the direction.
*
* If the Direction is Forward, the internal counter counts up and a positiv speed will be returned.
* If the Direction is Backward, the internal counter counts down and a negativ speed will be returned.
* If the Direction is None, no measurement will be taken.
*/
enum Direction
{
None,
Forward,
Backward
};
/**
* @brief Construct a new Speedometer object
*
* @param pin Pin on the Esp from the encoder.
* @param diameter Diameter of the wheel in meters.
* @param steps Encodersteps for a complete wheel rotation.
*/
Speedometer(uint8_t pin, double diameter, uint16_t steps);
/**
* @brief Construct a new Speedometer object
*
* @param pin Pin on the Esp from the encoder.
* @param diameter Diameter of the wheel in meters.
* @param steps Encodersteps for a complete wheel rotation.
*/
Speedometer(uint8_t pin, double diameter, uint16_t steps);
~Speedometer();
~Speedometer();
/**
* @brief Set the direction
*
* @param dir Direction
*/
void setDirection(Direction dir);
/**
* @brief Set the direction
*
* @param dir Direction
*/
void setDirection(Direction dir);
/**
* @brief Set the Enc Filter to prevent bouncing
*
* ignore pulses less than val x 2.5ns
*
* @param val default = 1000, max = 1023
*/
void setEncFilter(uint16_t val);
/**
* @brief Set the Enc Filter to prevent bouncing
*
* ignore pulses less than val x 2.5ns
*
* @param val default = 1000, max = 1023
*/
void setEncFilter(uint16_t val);
/**
* @brief Get the Direction
*
* @return Direction
*/
Direction getDirection() const { return this->currentDirection; }
/**
* @brief Get the Direction
*
* @return Direction
*/
Direction getDirection() const { return this->currentDirection; }
/**
* @brief Get the calculated speed of the Wheel
*
* @return double speed in m/s
*/
double getSpeed() const { return this->speed; }
double getSpeedRad() const { return this->rad; };
double getAvgSpeed() const;
/**
* @brief Get the calculated speed of the Wheel
*
* @return double speed in m/s
*/
double getSpeed() const { return this->speed; }
double getSpeedRad() const { return this->rad; };
double getAvgSpeed() const;
/**
* @brief Start calibration
*
* This functions stops the loop. So that steps of one manual wheel turn
* can measured. Call calibrationMeasurementStop to start the loop and get
* the result.
*/
void calibrationMeasurementStart();
/**
* @brief Start calibration
*
* This functions stops the loop. So that steps of one manual wheel turn
* can measured. Call calibrationMeasurementStop to start the loop and get
* the result.
*/
void calibrationMeasurementStart();
/**
* @brief Stop calibration
*
* Start the loop function and read the past steps.
*
* @return uint16_t steps since calibrationMeasurementStart was called
*/
uint16_t calibrationMeasurementStop();
/**
* @brief Stop calibration
*
* Start the loop function and read the past steps.
*
* @return uint16_t steps since calibrationMeasurementStart was called
*/
uint16_t calibrationMeasurementStop();
private:
void run() override;
void clearAvgBuf();
void addValToBuf(int16_t val);
int16_t calcAverage() const;
private:
void run() override;
void clearAvgBuf();
void addValToBuf(int16_t val);
int16_t calcAverage() const;
static constexpr uint8_t loopDelay = 30;
static constexpr uint8_t bufSize = 5;
static constexpr uint8_t conversionFactor = 100;
static constexpr uint8_t loopDelay = 30;
static constexpr uint8_t bufSize = 5;
static constexpr uint8_t conversionFactor = 100;
Counter *pulseCounter;
Direction currentDirection = Direction::None;
Counter* pulseCounter;
Direction currentDirection = Direction::None;
bool calibrationRunning = false;
bool calibrationRunning = false;
double speed = 0;
double rad = 0;
double diameter;
double speed = 0;
double rad = 0;
double diameter;
uint8_t printCounter = 0;
uint8_t bufPos = 0;
uint16_t steps;
int16_t buf[Speedometer::bufSize];
uint32_t lastMillisCalc = 0;
uint8_t printCounter = 0;
uint8_t bufPos = 0;
uint16_t steps;
int16_t buf[Speedometer::bufSize];
uint32_t lastMillisCalc = 0;
static constexpr uint16_t maxFilterValue = 1023;
};
#endif // SPEEDOMETER_H
lib/Times/debugTimes.cpp
+23 -12
View File
@@ -4,42 +4,53 @@
* @brief Implemention of the class debugTimes.h.
* @version 0.1
* @date 2021-12-13
*
*
* @copyright Copyright (c) 2021
*
*
*/
#include "debugTimes.h"
bool DebugTimes::print = false;
bool DebugTimes::printWarning = true;
DebugTimes::DebugTimes() {
this->startTime = millis();
if (DebugTimes::printWarning) {
std::cout << std::endl << "Warning: DebugTimes is muuted, no times are be shown." << std::endl << std::endl;
DebugTimes::DebugTimes()
: startTime{millis()}
{
if (DebugTimes::printWarning)
{
std::cout << std::endl
<< "Warning: DebugTimes is muuted, no times are be shown." << std::endl
<< std::endl;
DebugTimes::printWarning = false;
}
}
void DebugTimes::restart() {
void DebugTimes::restart()
{
this->startTime = millis();
}
uint16_t DebugTimes::stop() {
const uint16_t DebugTimes::stop()
{
return millis() - this->startTime;
}
uint16_t DebugTimes::stopConsol(const char* name, uint16_t minTime) {
uint64_t time = millis() - this->startTime;
const uint16_t DebugTimes::stopConsol(const char *name, uint16_t minTime)
{
const uint64_t time = millis() - this->startTime;
if (time > minTime && DebugTimes::print)
{
std::cout << name << " needs " << time << " ms" << std::endl;
}
return time;
}
void DebugTimes::setConsolOutput(bool enable) {
void DebugTimes::setConsolOutput(bool enable)
{
if (enable == DebugTimes::print)
{
return;
}
DebugTimes::print = enable;
DebugTimes::printWarning = !enable;
lib/Times/debugTimes.h
+42 -41
View File
@@ -4,9 +4,9 @@
* @brief Inherits a class to measure times of functions.
* @version 0.1
* @date 2021-12-13
*
*
* @copyright Copyright (c) 2021
*
*
*/
#ifndef DEBUG_TIMES_H
@@ -20,57 +20,58 @@
/**
* @brief A class to measure times of functions.
*
*
* This simple class only save the value of the millis()
* function when you call the constructor or restart().
* To get the elapsed time call stop() or stopConsol().
*
*
* @warning This class is not very accurate
* It only give you the time in milliseconds.
*/
class DebugTimes {
public:
/**
* @brief Construct a new Debug Times object
* Starts to count milliseconds
*/
class DebugTimes
{
public:
/**
* @brief Construct a new Debug Times object
* Starts to count milliseconds
*/
DebugTimes();
/**
* @brief Set the counter to 0
*/
DebugTimes();
/**
* @brief Set the counter to 0
*/
void restart();
void restart();
/**
* @brief Give the elapsed time
*
* @return uint16_t elapsed milliseconds
*/
uint16_t stop();
/**
* @brief Give the elapsed time
*
* @return uint16_t elapsed milliseconds
*/
const uint16_t stop();
/**
* @brief Print the elapsed time to consol
*
* @param name Functionname to print
* @param minTime A minimum time before printing
*
* @return uint16_t elapsed milliseconds
*/
uint16_t stopConsol(const char* name, uint16_t minTime = 0);
/**
* @brief Print the elapsed time to consol
*
* @param name Functionname to print
* @param minTime A minimum time before printing
*
* @return uint16_t elapsed milliseconds
*/
const uint16_t stopConsol(const char *name, uint16_t minTime = 0);
/**
* @brief Sets if the result should be printed.
*
* @param enable
*/
static void setConsolOutput(bool enable);
/**
* @brief Sets if the result should be printed.
*
* @param enable
*/
static void setConsolOutput(bool enable);
private:
uint64_t startTime;
private:
uint64_t startTime;
static bool print;
static bool printWarning;
static bool print;
static bool printWarning;
};
#endif //DEBUG_TIMES_H
#endif // DEBUG_TIMES_H
lib/calibrateCompass/calibrateCompass.cpp
+22 -20
View File
@@ -12,8 +12,8 @@
#include "calibrateCompass.h"
CalibrateCompass::CalibrateCompass(QMC5883LCompass *compass)
: compass{compass}
{
this->compass = compass;
this->state = State::Ready;
this->clearData();
this->activateOnlyChilds();
@@ -29,9 +29,9 @@ void CalibrateCompass::runAsChild()
bool changed = false;
this->compass->read();
int xAxis = this->compass->getX();
int yAxis = this->compass->getY();
int zAxis = this->compass->getZ();
const int xAxis = this->compass->getX();
const int yAxis = this->compass->getY();
const int zAxis = this->compass->getZ();
if (xAxis < this->data.data[0][0])
{
@@ -70,7 +70,9 @@ void CalibrateCompass::runAsChild()
}
if (changed)
{
this->lastChange = millis();
}
if (millis() - this->lastChange > this->maxTimeWithoutChange)
{
@@ -182,23 +184,23 @@ void CalibrateCompass::checkDataValidity()
sum += this->data.data[i][0];
sum += this->data.data[i][1];
}
this->dataValid = sum;
this->dataValid = static_cast<bool>(sum);
}
std::ostream &operator<<(std::ostream &os, const CalibrateCompass &caliComp)
std::ostream &operator<<(std::ostream &stream, const CalibrateCompass &caliComp)
{
os << "(";
os << caliComp.data.data[0][0];
os << ", ";
os << caliComp.data.data[0][1];
os << ", ";
os << caliComp.data.data[1][0];
os << ", ";
os << caliComp.data.data[1][1];
os << ", ";
os << caliComp.data.data[2][0];
os << ", ";
os << caliComp.data.data[2][1];
os << ")";
return os;
stream << "(";
stream << caliComp.data.data[0][0];
stream << ", ";
stream << caliComp.data.data[0][1];
stream << ", ";
stream << caliComp.data.data[1][0];
stream << ", ";
stream << caliComp.data.data[1][1];
stream << ", ";
stream << caliComp.data.data[2][0];
stream << ", ";
stream << caliComp.data.data[2][1];
stream << ")";
return stream;
}
lib/calibrateCompass/calibrateCompass.h
+37 -34
View File
@@ -1,12 +1,12 @@
/**
* @file calibrateCompass.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief
* @version 0.1
* @date 2023-05-23
*
*
* @copyright Copyright (c) 2023
*
*
*/
#pragma once
@@ -17,44 +17,47 @@
#include "component.h"
class CalibrateCompass : public Component {
public:
enum State {
Ready,
Calibrating,
Finished
};
class CalibrateCompass : public Component
{
public:
enum State
{
Ready,
Calibrating,
Finished
};
struct CallibrationData {
int data[3][2];
};
struct CallibrationData
{
int data[3][2];
};
CalibrateCompass(QMC5883LCompass* compass);
CalibrateCompass(QMC5883LCompass *compass);
void start();
void useData();
void removeCalibration();
void reset();
void saveData();
void loadData();
void start();
void useData();
void removeCalibration();
void reset();
void saveData();
void loadData();
State getState() const { return this->state; }
CallibrationData getCallibrationData() const { return this->data; }
State getState() const { return this->state; }
CallibrationData getCallibrationData() const { return this->data; }
friend std::ostream& operator<<(std::ostream& os, const CalibrateCompass& caliComp);
friend std::ostream &operator<<(std::ostream &stream, const CalibrateCompass &caliComp);
private:
void runAsChild() override;
void run() override;
void checkDataValidity();
private:
void runAsChild() override;
void run() override;
void checkDataValidity();
QMC5883LCompass* compass;
State state;
CallibrationData data;
QMC5883LCompass *compass;
State state = State::Ready;
CallibrationData data{};
void clearData();
void clearData();
bool dataValid = false;
const uint16_t maxTimeWithoutChange = 10000;
uint32_t lastChange = 0;
bool dataValid = false;
const uint16_t maxTimeWithoutChange = 10000;
uint32_t lastChange = 0;
};