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

Merge branch 'main' into projektarbeit

Browse Source
This commit is contained in:
kleiax
2023-10-12 20:35:31 +02:00
parent f78804d836 451517e7cd
commit e511afb2a5
80 changed files with 5031 additions and 3385 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Battery/battery.cpp
+180 -89
View File
@@ -4,144 +4,235 @@
* @brief Contains the implementation of the class Battery
* @version 0.1
* @date 2022-02-05
*
*
* @copyright Copyright (c) 2022
*
*
*/
#include "battery.h"
Battery::Battery(uint8_t pin, uint32_t r1, uint32_t r2) {
this->pin = pin;
this->r1 = r1;
this->r2 = r2;
Battery::Battery(uint8_t pin, uint32_t firstResistor, uint32_t secondResistor)
: pin{pin}, firstResistor{firstResistor}, secondResistor{secondResistor},
batteryVoltageFactor{firstResistor + secondResistor / static_cast<double>(secondResistor)}
{
this->initBuffer();
this->loopDelay = 100;
Component::loopDelay = Battery::loopDelay;
}
Battery::Battery(uint8_t pin) {
this->pin = pin;
Battery::Battery(uint8_t pin)
: pin{pin}
{
this->initBuffer();
this->loopDelay = 100;
Component::loopDelay = Battery::loopDelay;
}
void Battery::run() {
this->readAdcToBuf();
this->loopCounter++;
if (this->loopCounter == this->calulationDelayMultiplier) {
this->calculateBatteryVoltage();
this->calculateBatteryPercent();
this->loopCounter = 0;
this->calculatetNewValues = true;
}
return;
}
double Battery::getBatteryVoltage() const {
double res = this->batteryVoltage;
return (int)(res*100+0.5)/100.0;
}
bool Battery::isBatteryLow(double voltage) const {
if (this->getBatteryVoltage() <= voltage && this->batteryVoltage > this->absurdLowVoltage)
return true;
return false;
}
bool Battery::isNewValue() {
if (!this->calculatetNewValues)
return false;
this->calculatetNewValues = false;
return true;
}
double Battery::calculateInputVoltage() {
// Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095
double reading = this->getBufAvg();
if(reading < 1 || reading > 4095) return 0;
return - 0.000000000000016 * pow(reading,4)
+ 0.000000000118171 * pow(reading,3)
- 0.000000301211691 * pow(reading,2)
+ 0.001109019271794 * reading
+ 0.034143524634089;
}
void Battery::calculateBatteryVoltage() {
if (this->r1 && this->r2) {
this->batteryVoltage = (this->calculateInputVoltage() * (double) (this->r1 + this->r2)) / (double) this->r2;
void Battery::run()
{
if (this->calibrationState != CalibrationState::None)
{
this->runCalibration();
return;
}
uint16_t adcValue = this->getBufAvg();
if (adcValue < this->rawAdcVoltages[0]) {
this->readAdcToBuf();
this->loopCounter++;
if (this->loopCounter >= this->calculateDelayMultiplier)
{
this->calculateBatteryVoltage();
this->calculateBatteryPercent();
this->loopCounter = 0;
this->calculatedNewValues = true;
}
}
void Battery::runCalibration()
{
if (this->calibrationState != CalibrationState::Reading)
{
return;
}
this->readAdcToBuf();
if (this->bufferPos == 0)
{
const uint16_t res = this->getBufAvg();
this->newRawAdcVoltages[this->currentCalibrationVoltage] = res;
std::cout << "Index: "
<< (int)this->currentCalibrationVoltage
<< " Value: "
<< (int)res
<< std::endl;
this->currentCalibrationVoltage++;
this->calibrationState = CalibrationState::Waiting;
if (this->currentCalibrationVoltage == Battery::rawAdcVoltagesCount)
{
this->calibrationState = CalibrationState::Finished;
}
}
}
double Battery::getBatteryVoltage() const
{
return static_cast<int>((this->batteryVoltage * 100 + 0.5)) / 100.0;
}
bool Battery::isBatteryLow(double voltage) const
{
if (this->getBatteryVoltage() <= voltage && this->batteryVoltage > this->absurdLowVoltage)
{
return true;
}
return false;
}
bool Battery::isNewValue()
{
if (!this->calculatedNewValues)
{
return false;
}
this->calculatedNewValues = false;
return true;
}
void Battery::nextVoltageIsReady()
{
if (this->calibrationState == CalibrationState::Waiting)
{
this->calibrationState = CalibrationState::Reading;
}
}
void Battery::startCalibration()
{
this->calibrationState = CalibrationState::Waiting;
this->currentCalibrationVoltage = 0;
this->bufferPos = 0;
Component::loopDelay = Battery::loopDelay / 2;
this->newRawAdcVoltages = new uint16_t[Battery::rawAdcVoltagesCount];
}
void Battery::finishCalibration()
{
if (this->calibrationState != CalibrationState::None)
{
return;
}
delete[] this->newRawAdcVoltages;
this->calibrationState = CalibrationState::None;
Component::loopDelay = Battery::loopDelay;
}
double Battery::calculateInputVoltage()
{
// Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095
double reading = this->getBufAvg();
if (reading < 1 || reading > Battery::adcMaxValue)
{
return 0;
}
return -this->adcCurveCoefficient[0] * pow(reading, 4) + this->adcCurveCoefficient[1] * pow(reading, 3) - this->adcCurveCoefficient[2] * pow(reading, 2) + this->adcCurveCoefficient[3] * reading + this->adcCurveCoefficient[4];
}
void Battery::calculateBatteryVoltage()
{
if (this->firstResistor && this->secondResistor)
{
this->batteryVoltage = this->calculateInputVoltage() * this->batteryVoltageFactor;
return;
}
const uint16_t adcValue = this->getBufAvg();
if (adcValue < this->rawAdcVoltages[0])
{
this->batteryVoltage = -1;
return;
}
if (adcValue > this->rawAdcVoltages[this->rawAdcVoltagesCount] + 50) {
this->batteryVoltage = -2;
return;
}
uint8_t index;
for (index = 1; index < this->rawAdcVoltagesCount; index++) {
uint8_t index = 1;
for (; index < this->rawAdcVoltagesCount; index++)
{
if (adcValue < this->rawAdcVoltages[index])
{
break;
}
}
double indexDelta = this->rawAdcVoltages[index] - this->rawAdcVoltages[index - 1];
double valueDelta = this->rawAdcVoltages[index] - adcValue;
const double indexDelta = this->rawAdcVoltages[index] - this->rawAdcVoltages[index - 1];
const double valueDelta = this->rawAdcVoltages[index] - adcValue;
double voltage = this->startVoltage + (index - 1) * this->stepVoltage;
voltage += valueDelta / indexDelta * this->stepVoltage;
this->batteryVoltage = voltage;
// std::cout << "Battery::calculateBatteryVoltage() - Voltage: " << voltage << " Index: " <<(int) index << " adcValue: " << (int) adcValue <<" iD: " << indexDelta << " vD: " << valueDelta << std::endl;
}
void Battery::calculateBatteryPercent() {
void Battery::calculateBatteryPercent()
{
int8_t size = sizeof(this->capacityVoltages) / sizeof(*this->capacityVoltages);
uint8_t i;
for (i = 0; i < size; i++) {
if (this->batteryVoltage <= this->capacityVoltages[i])
uint8_t index = 0;
for (; index < size; index++)
{
if (this->batteryVoltage <= this->capacityVoltages[index])
{
break;
}
}
if (i == 0) {
if (this->batteryVoltage > 6)
std::cout << "Critical low battery!" << std::endl;
} else if (i == size - 1) {
} else {
double diffToLowerVal = this->batteryVoltage - this->capacityVoltages[i - 1];
double diffToHigherVal = this->capacityVoltages[i] - this->batteryVoltage;
if (diffToLowerVal > diffToHigherVal)
i--;
if (index == 0)
{
if (this->batteryVoltage > this->absurdLowVoltage)
{
std::cout << "Critical low battery!" << std::endl;
}
}
this->batteryPercent = i * (100 / (size - 1));
else if (index == size - 1)
{
}
else
{
const double diffToLowerVal = this->batteryVoltage - this->capacityVoltages[index - 1];
const double diffToHigherVal = this->capacityVoltages[index] - this->batteryVoltage;
if (diffToLowerVal > diffToHigherVal)
{
index--;
}
}
this->batteryPercent = index * (100 / (size - 1));
}
void Battery::readAdcToBuf() {
void Battery::readAdcToBuf()
{
this->adcBuffer[this->bufferPos] = analogRead(this->pin);
this->bufferPos++;
if (this->bufferPos == Battery::bufferSize)
{
this->bufferPos = 0;
}
// std::cout << "Battery::readAdcToBuf added Value: " << this->adcBuffer[this->bufferPos] << std::endl;
}
void Battery::initBuffer() {
void Battery::initBuffer()
{
for (uint8_t i = 0; i < Battery::bufferSize; i++)
{
this->adcBuffer[i] = 0;
}
}
uint16_t Battery::getBufAvg() const {
uint16_t Battery::getBufAvg() const
{
uint32_t res = 0;
uint8_t emptyPos = 0;
for (uint8_t i = 0; i < Battery::bufferSize; i++) {
for (uint8_t i = 0; i < Battery::bufferSize; i++)
{
if (this->adcBuffer[i] == 0)
{
emptyPos++;
}
res += this->adcBuffer[i];
}
return res / (Battery::bufferSize - emptyPos);
lib/Battery/battery.h
+165 -81
View File
@@ -4,9 +4,9 @@
* @brief Contains a class for battery monitoring
* @version 0.1
* @date 2022-02-05
*
*
* @copyright Copyright (c) 2022
*
*
*/
#ifndef BATTERY_H
@@ -21,98 +21,182 @@
/**
* @brief A class for battery monitoring
*
*
* This class reads the voltage from an analog pin to calculate the
* charge level of a 3 Cell Li-Poly battery pack. The battery pack have
* to be after a voltage diveder, so that maximum voltage for the
* to be after a voltage divider, so that maximum voltage for the
* microcontroller is 3.3 Volt.
*/
class Battery : public Component {
public:
/**
* @brief Construct a new Battery object
*
* The voltage devider have to be calculated, so that the input
* voltage from 3.3 Volt is never exceeded. It is assumed that
* the microcontroller is connected to the second resistor.
*
* @param pin The analog to read from.
* @param r1 First resistor of the voltage devider.
* @param r2 Second resistor of the voltage devider.
*/
Battery(uint8_t pin, uint32_t r1, uint32_t r2);
Battery(uint8_t pin);
class Battery : public Component
{
public:
/**
* @brief States when the calibration modes is active
*
* - None means that no calibration is running
* - Reading means that the adc takes multiple values to calculate an average
* - Waiting means that the user has to set the new wanted voltage
* - Finished means that all measurements was taken
*/
enum CalibrationState
{
None,
Reading,
Waiting,
Finished
};
/**
* @brief Get the battery voltage
*
* @return double in Volt
*/
double getBatteryVoltage() const;
/**
* @brief Construct a new Battery object
*
* The voltage divider have to be calculated, so that the input
* voltage from 3.3 Volt is never exceeded. It is assumed that
* the microcontroller is connected to the second resistor.
*
* @param pin The analog to read from.
* @param firstResistor First resistor of the voltage divider.
* @param secondResistor Second resistor of the voltage divider.
*/
Battery(uint8_t pin, uint32_t firstResistor, uint32_t secondResistor);
/**
* @brief Get the charge level of the battery
*
* @return uint8_t charge level in percent
*/
uint8_t getBatteryPercent() const { return this->batteryPercent; }
/**
* @brief Construct a new Battery object
*
* With this constructor the real voltage is not calculated with the
* voltage divider but with a table which contains the raw reading from
* the adc mapped to a specific voltage
*
* @param pin
*/
Battery(uint8_t pin);
/**
* @brief Checks if the battery is low.
*
* The function will also return false if the battery voltage is
* absurd low. This is for the case that the uController is powered
* by usb and no battery is connected.
*
* @param voltage the limit the battery have to
* @return true if the battery is low
* @return false if the battery is high
*/
bool isBatteryLow(double voltage) const;
/**
* @brief Get the battery voltage
*
* @return double in Volt
*/
double getBatteryVoltage() const;
bool isNewValue();
/**
* @brief Get the charge level of the battery
*
* @return uint8_t charge level in percent
*/
uint8_t getBatteryPercent() const { return this->batteryPercent; }
private:
void run() override;
double calculateInputVoltage();
void calculateBatteryVoltage();
void calculateBatteryPercent();
void readAdcToBuf();
void initBuffer();
uint16_t getBufAvg() const;
/**
* @brief Checks if the battery is low.
*
* The function will also return false if the battery voltage is
* absurd low. This is for the case that the uController is powered
* by usb and no battery is connected.
*
* @param voltage the limit the battery have to
* @return true if the battery is low
* @return false if the battery is high
*/
bool isBatteryLow(double voltage) const;
static const uint8_t bufferSize = 30;
/**
* @brief Check if a new voltage was been calculated
*
* @return true
* @return false
*/
bool isNewValue();
uint8_t absurdLowVoltage = 5;
uint8_t pin;
uint8_t batteryPercent = 0;
uint8_t batteryLowPercent = 10;
uint8_t bufferPos = 0;
uint8_t calulationDelayMultiplier = 10;
uint8_t loopCounter = 0;
uint16_t adcBuffer[bufferSize];
uint32_t r1 = 0;
uint32_t r2 = 0;
bool calculatetNewValues = false;
double batteryVoltage = 0;
// Calibration
CalibrationState getCalibrationState() const { return this->calibrationState; }
const float capacityVoltages[21] = {9.82, 10.83, 11.06, 11.12, // 0 5 10 15
11.18, 11.24, 11.3, 11.36, // 20 25 30 35
11.39, 11.45, 11.51, 11.56, // 40 45 50 55
11.62, 11.74, 11.86, 11.95, // 60 65 70 75
12.07, 12.25, 12.33, 12.45, // 80 85 90 95
12.6 };
/**
* @brief Get the current calibration voltage target
*
* The returned value stand for the index of the table for this reason
* the real value have to be calculated. After the returned voltage has been set
* you have to call nextVoltageIsReady().
*
* @return uint8_t voltage multiply with 0,1 and add 7
*/
uint8_t getCurrentCalibrationVoltage() const { return this->currentCalibrationVoltage; }
const uint8_t rawAdcVoltagesCount = 60;
const double startVoltage = 7;
const double stepVoltage = 0.1;
const uint16_t rawAdcVoltages[60] = // from 7.0V to 12.9V in 0.1V steps
{1820, 1851, 1880, 1910, 1937, 1967, 1992, 2020, 2048, 2080,
2109, 2136, 2163, 2189, 2218, 2244, 2273, 2302, 2334, 2363,
2391, 2415, 2441, 2471, 2499, 2531, 2557, 2587, 2617, 2646,
2674, 2699, 2730, 2761, 2791, 2816, 2843, 2872, 2900, 2930,
2958, 2991, 3017, 3049, 3080, 3115, 3144, 3178, 3208, 3242,
3276, 3313, 3346, 3389, 3433, 3470, 3509, 3548, 3590, 3636};
/**
* @brief Read next wanted voltage
*
* If this function is called, the calibration mode reads the new voltage
* and save the value in the table.
*/
void nextVoltageIsReady();
/**
* @brief Calibrate the battery readings
*
* This calibration has only an effect if the Component
* uses the table with the raw adc values. The calibration gives
* the user different voltages that have to be set with a
* laboratory power supply. The power supply have to be connected
* instead of the battery.
*/
void startCalibration();
/**
* @brief abort the calibration
*/
void finishCalibration();
private:
void run() override;
void runCalibration();
double calculateInputVoltage();
void calculateBatteryVoltage();
void calculateBatteryPercent();
void readAdcToBuf();
void initBuffer();
uint16_t getBufAvg() const;
static constexpr uint8_t bufferSize = 30;
static constexpr uint8_t loopDelay = 100;
static constexpr uint16_t adcMaxValue = 4095;
CalibrationState calibrationState = CalibrationState::None;
uint8_t absurdLowVoltage = 5;
uint8_t pin;
uint8_t batteryPercent = 0;
uint8_t batteryLowPercent = 10;
uint8_t bufferPos = 0;
uint8_t calculateDelayMultiplier = 5;
uint8_t loopCounter = 0;
uint8_t currentCalibrationVoltage = 0; // *0.1 + 7
uint16_t adcBuffer[bufferSize];
uint16_t *newRawAdcVoltages = nullptr;
uint32_t firstResistor = 0;
uint32_t secondResistor = 0;
bool calculatedNewValues = false;
double batteryVoltage = 0;
double batteryVoltageFactor;
const float capacityVoltages[21] = {9.82, 10.83, 11.06, 11.12, // 0 5 10 15
11.18, 11.24, 11.3, 11.36, // 20 25 30 35
11.39, 11.45, 11.51, 11.56, // 40 45 50 55
11.62, 11.74, 11.86, 11.95, // 60 65 70 75
12.07, 12.25, 12.33, 12.45, // 80 85 90 95
12.6};
static constexpr uint8_t rawAdcVoltagesCount = 60;
const double startVoltage = 7;
const double stepVoltage = 0.1;
const uint16_t rawAdcVoltages[rawAdcVoltagesCount] = // from 7.0V to 12.9V in 0.1V steps
{1992, 2021, 2056, 2090, 2118, 2145, 2177, 2208, 2241, 2272,
2298, 2331, 2362, 2387, 2420, 2453, 2482, 2514, 2543, 2577,
2607, 2640, 2670, 2703, 2736, 2763, 2794, 2826, 2858, 2890,
2920, 2956, 2983, 3019, 3054, 3088, 3121, 3158, 3189, 3226,
3264, 3300, 3339, 3379, 3414, 3453, 3500, 3544, 3598, 3636,
3682, 3730, 3781, 3837, 3887, 3943, 3997, 4054, 4093, 4095};
const double adcCurveCoefficient[5] = {0.000000000000016,
0.000000000118171,
0.000000301211691,
0.001109019271794,
0.034143524634089};
};
#endif // BATTERY_H
lib/CalcAzimuth/calcAzimuth.cpp
+68 -39
View File
@@ -1,92 +1,121 @@
/**
* @file calcAzimuth.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief
* @version 0.1
* @date 2023-09-03
*
*
* @copyright Copyright (c) 2023
*
*
*/
#include "calcAzimuth.h"
CalcAzimuth::CalcAzimuth(Point point) {
this->lastChangePoint = point;
this->currentPosition = point;
this->loopDelay = 50;
CalcAzimuth::CalcAzimuth(Point point)
: lastChangePoint{point}, currentPosition{point}
{
Component::loopDelay = CalcAzimuth::loopDelay;
}
void CalcAzimuth::drivingDirectionChange(Point point) {
if (point.isInit() && point.isValid()) {
void CalcAzimuth::drivingDirectionChange(Point point)
{
if (point.isInit() && point.isValid())
{
this->directionChangeMode = true;
this->lastChangePoint = point;
this->state = State::Invalid;
}
}
void CalcAzimuth::updateCurrentPosition(Point point) {
void CalcAzimuth::updateCurrentPosition(Point point)
{
this->currentPosition = point;
this->positionChanged = true;
}
void CalcAzimuth::run() {
String CalcAzimuth::stateToString(State state)
{
switch (state)
{
case State::Invalid:
return "Invalid";
case State::Bad:
return "Bad";
case State::Ok:
return "Ok";
case State::Good:
return "Good";
case State::Super:
return "Super";
default:
return "UNKOWN";
}
}
void CalcAzimuth::run()
{
if (!this->positionChanged)
{
return;
}
this->positionChanged = false;
this->updateAzimuth();
}
void CalcAzimuth::updateAzimuth() {
if (!this->directionChangeMode
|| this->lastChangePoint.distanceTo(this->currentPosition) < 1.0)
void CalcAzimuth::updateAzimuth()
{
if (!this->directionChangeMode || this->lastChangePoint.distanceTo(this->currentPosition) < 1.0)
{
this->state = State::Invalid;
this->calcAzimuth = 999;
this->calcAzimuth = INT16_MIN;
return;
}
this->calcAzimuth = this->lastChangePoint.courseTo(this->currentPosition);
// Map point accuracy to State
if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM)
if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM)
{
this->state = State::Good;
}
else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM)
}
else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM)
{
this->state = State::Ok;
}
else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM)
}
else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM)
{
this->state = State::Bad;
}
else
}
else
{
this->state = State::Invalid;
}
// Upgrade quality if the range grows up
if (this->lastChangePoint.distanceTo(this->currentPosition) > 2.0) {
switch (this->state) {
case State::Bad :
this->state = State::Ok;
break;
case State::Ok :
this->state = State::Good;
break;
if (this->lastChangePoint.distanceTo(this->currentPosition) > this->minDistanceForBetterQuality)
{
switch (this->state)
{
case State::Bad:
this->state = State::Ok;
break;
case State::Good :
this->state = State::Super;
break;
case State::Ok:
this->state = State::Good;
break;
default:
break;
case State::Good:
this->state = State::Super;
break;
default:
break;
}
}
}
lib/CalcAzimuth/calcAzimuth.h
+71 -28
View File
@@ -1,12 +1,12 @@
/**
* @file calcAzimuth.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief Contains a class that calculates the azimuth from a last and a current position
* @version 0.1
* @date 2023-09-03
*
*
* @copyright Copyright (c) 2023
*
*
*/
#ifndef CALC_AZIMUTH_H
@@ -15,36 +15,79 @@
#include "component.h"
#include "point.h"
class CalcAzimuth : public Component {
public:
enum State {
Invalid,
Bad,
Ok,
Good,
Super
};
/**
* @brief A class to calculate an azimuth
*
* This class calculates the current Azimuth with the last position
* where the rover has been rotated and the current position
*/
class CalcAzimuth : public Component
{
public:
/**
* @brief States which represent the quality of the current calculated azimuth
*/
enum State
{
Invalid,
Bad,
Ok,
Good,
Super
};
CalcAzimuth(Point point);
/**
* @brief Construct a new Calc Azimuth object
*
* @param point current position
*/
CalcAzimuth(Point point);
void drivingDirectionChange(Point point);
void updateCurrentPosition(Point point);
void disableCalcAzimuth() { this->directionChangeMode = false; }
/**
* @brief Have to be called if the rover rotates
*
* @param point current position
*/
void drivingDirectionChange(Point point);
int16_t getAzimuth() const { return this->calcAzimuth; }
State getState() const { return this->state; }
/**
* @brief update the current position
*
* This function should be called if the rover has moved in
* a straight direction, to calculated the current Azimuth.
* More distance to the point given to drivingDirectionChange()
* increase the accuracy of the calculation.
*
* @param point current position
*/
void updateCurrentPosition(Point point);
void disableCalcAzimuth() { this->directionChangeMode = false; }
private:
void run() override;
void updateAzimuth();
int16_t getAzimuth() const { return this->calcAzimuth; }
State state = State::Invalid;
Point lastChangePoint;
Point currentPosition;
/**
* @brief Get the State struct
*
* @return State current quality of the calculation
*/
State getState() const { return this->state; }
bool positionChanged = false;
bool directionChangeMode = false;
int16_t calcAzimuth = INT16_MAX;
static String stateToString(State state);
private:
void run() override;
void updateAzimuth();
State state = State::Invalid;
Point lastChangePoint;
Point currentPosition;
bool positionChanged = false;
bool directionChangeMode = false;
int16_t calcAzimuth = INT16_MAX;
double minDistanceForBetterQuality = 2;
static constexpr uint8_t loopDelay = 50;
};
#endif //CALC_AZIMUTH_H
#endif // CALC_AZIMUTH_H
lib/calibrateCompass/calibrateCompass.cpp → lib/CalibrateCompass/calibrateCompass.cpp
+88 -61
View File
@@ -1,68 +1,81 @@
/**
* @file calibrateCompass.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief
* @version 0.1
* @date 2023-05-23
*
*
* @copyright Copyright (c) 2023
*
*
*/
#include "calibrateCompass.h"
CalibrateCompass::CalibrateCompass(QMC5883LCompass* compass) {
this->compass = compass;
CalibrateCompass::CalibrateCompass(QMC5883LCompass *compass)
: compass{compass}
{
this->state = State::Ready;
this->clearData();
this->activateOnlyChilds();
}
void CalibrateCompass::runAsChild() {
void CalibrateCompass::runAsChild()
{
if (this->state != State::Calibrating)
{
return;
}
bool changed = false;
this->compass->read();
int x = this->compass->getX();
int y = this->compass->getY();
int z = this->compass->getZ();
const int xAxis = this->compass->getX();
const int yAxis = this->compass->getY();
const int zAxis = this->compass->getZ();
if(x < this->data.data[0][0]) {
this->data.data[0][0] = x;
if (xAxis < this->data.data[0][0])
{
this->data.data[0][0] = xAxis;
changed = true;
}
if(x > this->data.data[0][1]) {
this->data.data[0][1] = x;
if (xAxis > this->data.data[0][1])
{
this->data.data[0][1] = xAxis;
changed = true;
}
if(y < this->data.data[1][0]) {
this->data.data[1][0] = y;
if (yAxis < this->data.data[1][0])
{
this->data.data[1][0] = yAxis;
changed = true;
}
if(y > this->data.data[1][1]) {
this->data.data[1][1] = y;
if (yAxis > this->data.data[1][1])
{
this->data.data[1][1] = yAxis;
changed = true;
}
if(z < this->data.data[2][0]) {
this->data.data[2][0] = z;
if (zAxis < this->data.data[2][0])
{
this->data.data[2][0] = zAxis;
changed = true;
}
if(z > this->data.data[2][1]) {
this->data.data[2][1] = z;
if (zAxis > this->data.data[2][1])
{
this->data.data[2][1] = zAxis;
changed = true;
}
if (changed)
{
this->lastChange = millis();
}
if (millis() - this->lastChange > this->maxTimeWithoutChange) {
if (millis() - this->lastChange > this->maxTimeWithoutChange)
{
this->state = State::Finished;
this->checkDataValidity();
}
@@ -70,9 +83,12 @@ void CalibrateCompass::runAsChild() {
void CalibrateCompass::run() {}
void CalibrateCompass::start() {
void CalibrateCompass::start()
{
if (this->state != State::Ready)
{
return;
}
this->clearData();
this->state = State::Calibrating;
@@ -80,39 +96,45 @@ void CalibrateCompass::start() {
this->lastChange = millis();
}
void CalibrateCompass::useData() {
if (!this->dataValid) {
void CalibrateCompass::useData()
{
if (!this->dataValid)
{
std::cout << "CalibrateCompass::useData - Data not valid" << std::endl;
return;
}
this->compass->setCalibration( this->data.data[0][0],
this->data.data[0][1],
this->data.data[1][0],
this->data.data[1][1],
this->data.data[2][0],
this->data.data[2][1]
);
this->compass->setCalibration(this->data.data[0][0],
this->data.data[0][1],
this->data.data[1][0],
this->data.data[1][1],
this->data.data[2][0],
this->data.data[2][1]);
std::cout << "CalibrateCompass::useData " << *this << std::endl;
std::cout << "CalibrateCompass::useData " << *this << std::endl;
}
void CalibrateCompass::removeCalibration() {
void CalibrateCompass::removeCalibration()
{
this->compass->clearCalibration();
}
void CalibrateCompass::reset() {
void CalibrateCompass::reset()
{
this->clearData();
this->state = State::Ready;
}
void CalibrateCompass::saveData() {
void CalibrateCompass::saveData()
{
if (!this->dataValid)
{
return;
}
Preferences preferences;
preferences.begin("compass", false);
preferences.putInt("xLow", this->data.data[0][0]);
preferences.putInt("xHigh", this->data.data[0][1]);
preferences.putInt("yLow", this->data.data[1][0]);
@@ -123,7 +145,8 @@ void CalibrateCompass::saveData() {
preferences.end();
}
void CalibrateCompass::loadData() {
void CalibrateCompass::loadData()
{
Preferences preferences;
preferences.begin("compass", true);
@@ -138,42 +161,46 @@ void CalibrateCompass::loadData() {
this->checkDataValidity();
}
void CalibrateCompass::clearData() {
for (uint8_t i = 0; i < 3; i++) {
void CalibrateCompass::clearData()
{
for (uint8_t i = 0; i < 3; i++)
{
this->data.data[i][0] = 0;
this->data.data[i][1] = 0;
}
this->dataValid = false;
}
void CalibrateCompass::checkDataValidity() {
void CalibrateCompass::checkDataValidity()
{
int sum = 0;
for (uint8_t i = 0; i < 3; i++) {
if (this->data.data[i][0] > INT16_MAX || this->data.data[i][0] < INT16_MIN
|| this->data.data[i][1] > INT16_MAX || this->data.data[i][1] < INT16_MIN)
{
for (uint8_t i = 0; i < 3; i++)
{
if (this->data.data[i][0] > INT16_MAX || this->data.data[i][0] < INT16_MIN || this->data.data[i][1] > INT16_MAX || this->data.data[i][1] < INT16_MIN)
{
this->dataValid = false;
return;
}
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) {
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;
std::ostream &operator<<(std::ostream &stream, const CalibrateCompass &caliComp)
{
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
+110
View File
@@ -0,0 +1,110 @@
/**
* @file calibrateCompass.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains a class to calibrate the compass module
* @version 0.1
* @date 2023-05-23
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include <QMC5883LCompass.h>
#include <Preferences.h>
#include <iostream>
#include "component.h"
/**
* @brief A Class to calibrate the compass
*
* This class reads the raw values of the compass while
* the rove have to be moved. The lowest and highest values
* are used to calibrate the compass to the current location.
*/
class CalibrateCompass : public Component
{
public:
/**
* @brief State of the calibration process
*/
enum State
{
Ready,
Calibrating,
Finished
};
/**
* @brief Type for calibration data
*
* The data consist of 6 values. For each for the 3 axis
* are to integer needed.
*/
struct CalibrationData
{
int data[3][2];
};
CalibrateCompass(QMC5883LCompass *compass);
/**
* @brief Starts the calibration
*/
void start();
/**
* @brief Use the measured calibration data
*/
void useData();
/**
* @brief Remove the measured calibration data
*/
void removeCalibration();
/**
* @brief Reset the calibration process to start again
*/
void reset();
/**
* @brief Save the measured calibration data to the flash
*/
void saveData();
/**
* @brief Load the measured calibration data from the flash
*
*/
void loadData();
State getState() const { return this->state; }
CalibrationData getCalibrationData() const { return this->data; }
/**
* @brief Makes the calibration data printable with std::cout()
*
* @param stream
* @param caliComp
* @return std::ostream&
*/
friend std::ostream &operator<<(std::ostream &stream, const CalibrateCompass &caliComp);
private:
void runAsChild() override;
void run() override;
void checkDataValidity();
QMC5883LCompass *compass;
State state = State::Ready;
CalibrationData data{};
void clearData();
bool dataValid = false;
const uint16_t maxTimeWithoutChange = 10000;
uint32_t lastChange = 0;
};
lib/Component/component.cpp
+26 -12
View File
@@ -1,25 +1,35 @@
#include "component.h"
Component::Component(uint16_t loopDelay) {
this->loopDelay = loopDelay;
Component::Component(uint16_t loopDelay) : loopDelay{loopDelay}
{
}
void Component::loop() {
void Component::loop()
{
if (!this->active)
{
return;
if (this->childComponents.size()){
std::list<Component*>::iterator it;
for (it = this->childComponents.begin(); it != this->childComponents.end(); it++)
(*it)->loop();
}
this->runAsChild();
if (this->childComponents.size())
{
std::list<Component *>::iterator it;
for (it = this->childComponents.begin(); it != this->childComponents.end(); it++)
{
(*it)->loop();
}
}
this->runAsChild();F
if (this->onlyChilds)
{
return;
}
if (this->loopDelay && millis() - this->lastMillis < this->loopDelay)
if (static_cast<bool>(this->loopDelay) && millis() - this->lastMillis < this->loopDelay)
{
return;
}
this->lastMillis = millis();
@@ -28,13 +38,17 @@ void Component::loop() {
this->afterRun();
if (this->timeUpdateAfter)
{
this->lastMillis = millis();
}
}
void Component::addChildComponent(Component* child) {
void Component::addChildComponent(Component *child)
{
this->childComponents.push_back(child);
}
void Component::removeChildComponent(Component* child) {
void Component::removeChildComponent(Component *child)
{
this->childComponents.remove(child);
}
lib/Component/component.h
+122 -29
View File
@@ -1,12 +1,12 @@
/**
* @file component.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief Contains an interface to make non blocking components with delay.
* @version 0.1
* @date 2023-08-16
*
*
* @copyright Copyright (c) 2023
*
*
*/
#pragma once
@@ -15,37 +15,130 @@
#include <list>
class Component {
public:
Component() {}
Component(uint16_t loopDelay);
/**
* @brief An Interface to make components
*
* A component is a task that be called in a loop which not
* runs every loop, so every component has a non blocking delay.
*
* A component can manage other components which called children components.
*/
class Component
{
public:
/**
* @brief Construct a new Component object
*
* With the default constructor the created component is
* by default inactive. This does not affect the execution of
* the children components.
*/
Component() {}
void loop();
void deactivate() { this->active = false; }
void activate() { this->active = false; }
protected:
virtual void runAsChild() {}
virtual void beforeRun() {}
virtual void run() = 0;
virtual void afterRun() {}
/**
* @brief Construct a new Component object
*
* If the given parameter is zero, there are no differences to the
* default constructor.
*
* @param loopDelay the minimum time in milliseconds before the task runs
*/
Component(uint16_t loopDelay);
void addChildComponent(Component* child);
void removeChildComponent(Component* child);
/**
* @brief Runs the children components and the task
*
* The loop() function of the children is called every time this
* loop is called.
*
* The run() function which presents the task of this component is
* only called if the delay is reached.
*/
void loop();
void activateOnlyChilds() { this->onlyChilds = true; }
void deactivateOnlyChilds() { this->onlyChilds = false; }
/**
* @brief Deactivate this component
*
* If the component is deactivated the call of loop() ha no effect
*/
void deactivate() { this->active = false; }
void activate() { this->active = false; }
void setTimerAfterTask() { this->timeUpdateAfter = true; }
protected:
/**
* @brief Override this function to avoid the delay
*/
virtual void runAsChild() {}
uint16_t loopDelay = 0;
/**
* @brief Runs befor the run() function
*
* This function is only called, if the delay
* is reached.
* The function do nothing, except the function is overwritten
* by the class which inherits this class.
*/
virtual void beforeRun() {}
private:
std::list<Component*> childComponents;
/**
* @brief The actual task
*
* This function have to be overwritten by the inheriting class.
*/
virtual void run() = 0;
bool active = true;
bool onlyChilds = false;
bool timeUpdateAfter = false;
uint32_t lastMillis = 0;
/**
* @brief Runs after the run() function
*
* This function is only called, if the delay
* is reached.
* The function do nothing, except the function is overwritten
* by the class which inherits this class.
*/
virtual void afterRun() {}
/**
* @brief Adds a child component
*
* The child component will be called every time the loop() function
* is called.
*
* @param child
*/
void addChildComponent(Component *child);
void removeChildComponent(Component *child);
void activateOnlyChilds() { this->onlyChilds = true; }
/**
* @brief Skip the actual task
*
* Same as set the loopDelay to zero.
*/
void deactivateOnlyChilds() { this->onlyChilds = false; }
/**
* @brief Set the timer after task
*
* If this function is called once the measurement of the delay
* starts after task has finished. The default is, that the
* measurement begins at the start of the task.
*/
void setTimerAfterTask() { this->timeUpdateAfter = true; }
/**
* @brief the minimum time in milliseconds before the task runs
*
* If this value is zero, the functions beforeRun(), run() and
* afterRun() would not be called
*/
uint16_t loopDelay = 0;
private:
std::list<Component *> childComponents;
bool active = true;
bool onlyChilds = false;
bool timeUpdateAfter = false;
uint32_t lastMillis = 0;
};
lib/ControllPad/controlPad.cpp → lib/ControlPad/controlPad.cpp
+38 -22
View File
@@ -1,37 +1,43 @@
/**
* @file controlPad.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief
* @version 0.1
* @date 2023-03-30
*
*
* @copyright Copyright (c) 2023
*
*
*/
#include "controlPad.h"
ControlPad::ControlPad() {
this->loopDelay = 5;
}
ControlPad::ControlPad() : Component(10), controlInput{0, 0, 0, 0} {}
void ControlPad::run() {
if(!this->connected)
void ControlPad::run()
{
if (!this->connected)
{
return;
}
if (millis() - this->lastMessageReceive > this->disconnectTime) {
if (millis() - this->lastMessageReceive > this->disconnectTime)
{
this->connected = false;
this->controlInput.buttons = 0;
this->controlInput.x = 127;
this->controlInput.y = 127;
this->controlInput.x = UINT8_MAX / 2;
this->controlInput.y = UINT8_MAX / 2;
return;
}
if (this->lastButtons != controlInput.buttons)
{
this->updated = true;
}
if (this->menuControl && this->controlInput.buttons > 0 && this->updated) {
if (this->firstButtonPress) {
if (static_cast<bool>(this->menuControl) && this->controlInput.buttons > 0 && this->updated)
{
if (this->firstButtonPress)
{
this->menuControl->printMenu();
this->firstButtonPress = false;
std::cout << "ControlPad::loop - First menu print" << std::endl;
@@ -39,38 +45,48 @@ void ControlPad::run() {
}
if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Left))
{
this->menuControl->left();
}
else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Right))
{
this->menuControl->right();
}
else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Up))
{
this->menuControl->up();
}
else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Down))
{
this->menuControl->down();
}
else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Yes))
{
this->menuControl->yes();
}
else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::No))
{
this->menuControl->no();
}
this->updated = false;
this->lastButtons = controlInput.buttons;
}
return;
}
void ControlPad::insertData(const uint8_t *data) {
void ControlPad::insertData(const uint8_t *data)
{
this->connected = true;
this->lastMessageReceive = millis();
uint8_t lastCount = this->controlInput.counter + 1;
const uint8_t lastCount = this->controlInput.counter + 1;
memcpy(&(this->controlInput), data, sizeof(this->controlInput));
if (lastCount != this->controlInput.counter)
std::cout << "ControlPad::insertData counter wrong value" << std::endl;
if (this->controlInput.x > 127 - this->deadZoneX
&& this->controlInput.x < 127 + this->deadZoneX)
this->controlInput.x = 127;
if (this->controlInput.x > UINT8_MAX / 2 - this->deadZoneX && this->controlInput.x < UINT8_MAX / 2 + this->deadZoneX)
this->controlInput.x = UINT8_MAX / 2;
if (this->controlInput.y > 127 - this->deadZoneY
&& this->controlInput.y < 127 + this->deadZoneY)
this->controlInput.y = 127;
if (this->controlInput.y > UINT8_MAX / 2 - this->deadZoneY && this->controlInput.y < UINT8_MAX / 2 + this->deadZoneY)
this->controlInput.y = UINT8_MAX / 2;
}
lib/ControlPad/controlPad.h
+80
View File
@@ -0,0 +1,80 @@
/**
* @file controlPad.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains a class that gets the input data
* @version 0.1
* @date 2023-03-30
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include <menuControl.h>
#include <controlPadInput.h>
#include <component.h>
/**
* @brief A class to manage inputs
*
* This class converts the incoming data to the buttons and
* the axis from the joystick. The converted data will be send
* to the menu.
*/
class ControlPad : public Component
{
public:
ControlPad();
/**
* @brief Insert the incoming data to convert
*
* The data is converted to the ControlPadInput struct.
*
* @param data have to be 4 byte long
*/
void insertData(const uint8_t *data);
/**
* @brief Set the MenuControl object
*
* The MenuControl object is used to control the Menu.
*
* @see Menu
* @see ControlPadInput
*
* @param menuControl
*/
void setMenuControl(MenuControl *menuControl) { this->menuControl = menuControl; }
/**
* @brief Get the Control Pad Data
*
* The pointer holds the lates data from the ControlPad
*
* @return const ControlPadInput*
*/
const ControlPadInput *getControlPadDataPtr() const { return &this->controlInput; }
bool isControlPadConnected() const { return this->connected; }
private:
void run() override;
MenuControl *menuControl = nullptr;
ControlPadInput controlInput;
bool connected = false;
bool updated = false;
bool firstButtonPress = true;
uint8_t deadZoneX = 20;
uint8_t deadZoneY = 20;
uint8_t lastButtons = 0;
uint16_t disconnectTime = 100;
uint32_t lastMessageReceive = 0;
};
lib/ControlPad/controlPadInput.h
+79
View File
@@ -0,0 +1,79 @@
/**
* @file controlPadInput.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-03-30
*
* @copyright Copyright (c) 2023
*
*/
#ifndef CONTROL_PAD_INPUT_H
#define CONTROL_PAD_INPUT_H
#include <stdint.h>
struct ControlPadInput
{
uint8_t buttons;
uint8_t x;
uint8_t y;
uint8_t counter;
};
class ControlPadButton
{
public:
enum PadButton
{
Left = 4,
Right = 8,
Up = 2,
Down = 1,
Yes = 32,
No = 16,
Action = 64
};
static bool isControlPadButtonPressed(const ControlPadInput *input, PadButton button)
{
uint8_t buttonNum = input->buttons;
switch (button)
{
case PadButton::Left:
return buttonNum & (uint8_t)PadButton::Left;
break;
case PadButton::Right:
return buttonNum & (uint8_t)PadButton::Right;
break;
case PadButton::Up:
return buttonNum & (uint8_t)PadButton::Up;
break;
case PadButton::Down:
return buttonNum & (uint8_t)PadButton::Down;
break;
case PadButton::Yes:
return buttonNum & (uint8_t)PadButton::Yes;
break;
case PadButton::No:
return buttonNum & (uint8_t)PadButton::No;
break;
case PadButton::Action:
return buttonNum & (uint8_t)PadButton::Action;
break;
default:
return false;
;
}
}
};
#endif // CONTROL_PAD_INPUT_H
lib/ControllPad/controlPad.h
-48
View File
@@ -1,48 +0,0 @@
/**
* @file controlPad.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-03-30
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include <menuControl.h>
#include <controlPadInput.h>
#include <component.h>
class ControlPad : public Component {
public:
ControlPad();
void insertData(const uint8_t *data);
void setMenuControl(MenuControl* menuControl) { this->menuControl = menuControl; }
const ControlPadInput* getControlPadDataPtr() const { return &this->controlInput; }
bool isControlPadConnected() const { return this->connected; }
private:
void run() override;
MenuControl* menuControl = nullptr;
ControlPadInput controlInput;
bool connected = false;
bool updated = false;
bool firstButtonPress = true;
uint8_t deadZoneX = 20;
uint8_t deadZoneY = 20;
uint8_t lastButtons = 0;
uint16_t disconnectTime = 100;
uint32_t lastMessageReceive = 0;
};
lib/ControllPad/controlPadInput.h
-74
View File
@@ -1,74 +0,0 @@
/**
* @file controlPadInput.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-03-30
*
* @copyright Copyright (c) 2023
*
*/
#ifndef CONTROL_PAD_INPUT_H
#define CONTROL_PAD_INPUT_H
#include <stdint.h>
struct ControlPadInput {
uint8_t buttons;
uint8_t x;
uint8_t y;
uint8_t counter;
};
class ControlPadButton {
public:
enum PadButton {
Left = 4,
Right = 8,
Up = 2,
Down = 1,
Yes = 32,
No = 16,
Action = 64
};
static bool isControlPadButtonPressed(const ControlPadInput *input, PadButton button) {
uint8_t buttonNum = input->buttons;
switch (button) {
case PadButton::Left :
return buttonNum & (uint8_t) PadButton::Left;
break;
case PadButton::Right :
return buttonNum & (uint8_t) PadButton::Right;
break;
case PadButton::Up :
return buttonNum & (uint8_t) PadButton::Up;
break;
case PadButton::Down :
return buttonNum & (uint8_t) PadButton::Down;
break;
case PadButton::Yes :
return buttonNum & (uint8_t) PadButton::Yes;
break;
case PadButton::No :
return buttonNum & (uint8_t) PadButton::No;
break;
case PadButton::Action :
return buttonNum & (uint8_t) PadButton::Action;
break;
default:
return false;;
}
}
};
#endif // CONTROL_PAD_INPUT_H
lib/Counter/counter.cpp
+21 -14
View File
@@ -1,14 +1,14 @@
#include "counter.h"
#include <inttypes.h>
uint8_t Counter::amountOfCounter = 0;
Counter::Counter(uint8_t pin) {
this->pulsePin = pin;
this->unit = static_cast<pcnt_unit_t>(Counter::amountOfCounter);
if (Counter::amountOfCounter < 7)
Counter::Counter(uint8_t pin)
: pulsePin{pin}, unit{static_cast<pcnt_unit_t>(Counter::amountOfCounter)}
{
if (Counter::amountOfCounter <= Counter::maxCounter)
{
Counter::amountOfCounter++;
}
pcnt_config_t config;
config.unit = this->unit;
@@ -24,33 +24,40 @@ Counter::Counter(uint8_t pin) {
pcnt_unit_config(&config);
}
void Counter::pause() {
void Counter::pause()
{
pcnt_counter_pause(this->unit);
}
void Counter::resume() {
void Counter::resume()
{
pcnt_counter_resume(this->unit);
}
void Counter::clear() {
void Counter::clear()
{
pcnt_counter_clear(this->unit);
}
int16_t Counter::getValue() const {
int16_t res;
int16_t Counter::getValue() const
{
int16_t res = 0;
pcnt_get_counter_value(this->unit, &res);
return res;
}
void Counter::setFilterValue(uint16_t value) {
void Counter::setFilterValue(uint16_t value)
{
pcnt_set_filter_value(this->unit, value);
this->filterEnable();
}
void Counter::filterEnable() {
void Counter::filterEnable()
{
pcnt_filter_enable(this->unit);
}
void Counter::filterDisable() {
void Counter::filterDisable()
{
pcnt_filter_disable(this->unit);
}
lib/Counter/counter.h
+64 -17
View File
@@ -1,30 +1,77 @@
/**
* @file counter.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains a class that abstract the hardware counter from the esp32
* @version 0.1
* @date 2023-10-12
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include <Arduino.h>
#include <driver/pcnt.h>
#include <cinttypes>
class Counter {
public:
Counter(uint8_t pin);
/**
* @brief A class that abstract the hardware counter from the esp32
*/
class Counter
{
public:
/**
* @brief Construct a new Counter object
*
* @param pin with incoming pulses
*/
Counter(uint8_t pin);
void pause();
void resume();
void clear();
/**
* @brief Pause the pulse counting
*/
void pause();
int16_t getValue() const;
/**
* @brief Resume the pulse counting
*/
void resume();
void setFilterValue(uint16_t value);
void filterEnable();
void filterDisable();
/**
* @brief Start counting by zero again
*/
void clear();
private:
static constexpr int16_t highLimit = INT16_MAX;
static constexpr uint8_t lowLimit = 0;
/**
* @brief Get the counted pulses
*
* @return int16_t
*/
int16_t getValue() const;
static uint8_t amountOfCounter;
/**
* @brief Set the filter value
*
* The filter skip all pulses after a pulse for the filter time.
* The filter time depends on the frequency of the processor. The time
* for a whole tact multiplied with the filter value results the filter time.
*
* @param value max 1023
*/
void setFilterValue(uint16_t value);
bool initalised = false;
void filterEnable();
void filterDisable();
uint8_t pulsePin;
pcnt_unit_t unit;
private:
static constexpr int16_t highLimit = INT16_MAX;
static constexpr uint8_t lowLimit = 0;
static constexpr uint8_t maxCounter = 6;
static uint8_t amountOfCounter;
bool initalized = false;
uint8_t pulsePin;
pcnt_unit_t unit;
};
lib/LcdWrapper/LcdWrapper.cpp
+37 -19
View File
@@ -1,65 +1,83 @@
/**
* @file displayWrapper.cpp
* @file LcdWrapper.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains the implementation of the class LcdWrapper
* @version 0.1
* @date 2023-01-08
*
*
* @copyright Copyright (c) 2023
*
*
*/
#include "LcdWrapper.h"
LcdWrapper::LcdWrapper(LiquidCrystal_I2C* lcd) {
this->lcd = lcd;
LcdWrapper::LcdWrapper(LiquidCrystal_I2C *lcd)
: lcd{lcd}, changed{false}
{
this->clear();
this->changed = false;
}
void LcdWrapper::run() {
void LcdWrapper::run()
{
if (!this->changed)
{
return;
}
this->lcd->clear();
for (uint8_t i = 0; i < LcdWrapper::totalLines; i++) {
for (uint8_t i = 0; i < LcdWrapper::totalLines; i++)
{
this->lcd->setCursor(0, i);
this->lcd->print(this->data[i]);
this->lcd->print(static_cast<const char *>(this->data[i]));
}
if (this->callback)
if (static_cast<bool>(this->callback))
{
this->callback(this->data, LcdWrapper::totalLines, LcdWrapper::totalRows);
}
this->changed = false;
}
void LcdWrapper::clear() {
for (uint8_t i = 0; i < LcdWrapper::totalLines; i++) {
for (uint8_t j = 0; j < LcdWrapper::totalRows; j++) {
void LcdWrapper::clear()
{
for (uint8_t i = 0; i < LcdWrapper::totalLines; i++)
{
for (uint8_t j = 0; j < LcdWrapper::totalRows; j++)
{
data[i][j] = ' ';
}
}
this->changed = true;
}
void LcdWrapper::setCursor(uint8_t row, uint8_t line) {
void LcdWrapper::setCursor(uint8_t row, uint8_t line)
{
if (row > LcdWrapper::totalRows - 1)
{
row = LcdWrapper::totalRows - 1;
}
if (line > LcdWrapper::totalLines - 1)
{
line = LcdWrapper::totalLines - 1;
}
this->cursorRow = row;
this->cursorLine = line;
}
void LcdWrapper::print(const char *str) {
void LcdWrapper::print(const char *str)
{
uint8_t inputStringPosition = 0;
for (uint8_t i = this->cursorRow; i < LcdWrapper::totalRows; i++) {
for (uint8_t i = this->cursorRow; i < LcdWrapper::totalRows; i++)
{
if (str[inputStringPosition] == '\0')
{
break;
else
this->data[this->cursorLine][i] = str[inputStringPosition];
}
this->data[this->cursorLine][i] = str[inputStringPosition];
inputStringPosition++;
}
this->changed = true;
lib/LcdWrapper/LcdWrapper.h
+45 -45
View File
@@ -1,12 +1,12 @@
/**
* @file displayWrapper.h
* @file LcdWrapper.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains the LcdWrapper class
* @version 0.1
* @date 2023-01-08
*
*
* @copyright Copyright (c) 2023
*
*
*/
#ifndef LCD_WRAPPER_H
@@ -17,59 +17,59 @@
#include <displayWrapper.h>
#include <component.h>
typedef void (*LcdWrapperCallback) (const char data[][16], uint8_t lines, uint8_t rows);
typedef void (*LcdWrapperCallback)(const char data[][16], uint8_t lines, uint8_t rows);
/**
* @brief A class for the Menu class to print information
*
* @brief A class for the Menu class to print information
*
* This class inherits the DisplayWrapper class as a interface.
* The class takes the information to print from any thread. The
* The class takes the information to print from any thread. The
* loop function has to be called to print the data.
*/
class LcdWrapper : public DisplayWrapper, public Component {
public:
/**
* @brief Construct a new Lcd Wrapper object
*
* @param lcd
*/
LcdWrapper(LiquidCrystal_I2C* lcd);
class LcdWrapper : public DisplayWrapper, public Component
{
public:
/**
* @brief Construct a new Lcd Wrapper object
*
* @param lcd
*/
LcdWrapper(LiquidCrystal_I2C *lcd);
/**
* @brief Empty the buffer
*
*/
void clear() override;
/**
* @brief Empty the buffer
*/
void clear() override;
/**
* @brief Set point where data to be saved
*
* @param row
* @param line
*/
void setCursor(uint8_t row, uint8_t line) override;
void setCallback(LcdWrapperCallback callback) { this->callback = callback; }
/**
* @brief Set point where data to be saved
*
* @param row
* @param line
*/
void setCursor(uint8_t row, uint8_t line) override;
void setCallback(LcdWrapperCallback callback) { this->callback = callback; }
/**
* @brief Save the data to be printed
*
* @param str
*/
void print(const char *str) override;
/**
* @brief Save the data to be printed
*
* @param str
*/
void print(const char *str) override;
static constexpr uint8_t totalRows = 16;
static constexpr uint8_t totalLines = 2;
private:
void run() override;
static constexpr uint8_t totalRows = 16;
static constexpr uint8_t totalLines = 2;
LiquidCrystal_I2C* lcd;
LcdWrapperCallback callback = nullptr;
char data[LcdWrapper::totalLines][LcdWrapper::totalRows];
private:
void run() override;
uint8_t cursorRow = 0;
uint8_t cursorLine = 0;
LiquidCrystal_I2C *lcd;
LcdWrapperCallback callback = nullptr;
char data[LcdWrapper::totalLines][LcdWrapper::totalRows];
bool changed = false;
uint8_t cursorRow = 0;
uint8_t cursorLine = 0;
bool changed = false;
};
#endif // DISPLAY_WRAPPER_H
lib/MQTT/debugMqtt.cpp
+51 -36
View File
@@ -5,99 +5,115 @@
* @see debugMqtt.h
* @version 0.1
* @date 2021-12-13
*
*
* @copyright Copyright (c) 2021
*
*
*/
#include "debugMqtt.h"
PubSubClient* DebugMqtt::client;
PubSubClient *DebugMqtt::client;
Loglevel DebugMqtt::loglevel;
bool DebugMqtt::isInit = false;
char DebugMqtt::msg[MQTT_BUFFER_SIZE];
char DebugMqtt::topic[MQTT_BUFFER_SIZE];
DebugMqtt::DebugMqtt(const char* name, uint8_t bufSize) {
this->name = name;
DebugMqtt::DebugMqtt(const char *name, uint8_t bufSize)
: name{name}
{
if (bufSize != 0)
{
this->bufSize = bufSize;
}
this->buf = new char[this->bufSize];
}
DebugMqtt::~DebugMqtt() {
DebugMqtt::~DebugMqtt()
{
delete this->buf;
}
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);
void DebugMqtt::sendMsg(Loglevel loglevel, String topic, String 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) {
void DebugMqtt::sendMsg(Loglevel loglevel, String msg)
{
this->sendMsg(loglevel, "", msg);
}
void DebugMqtt::sendData(Loglevel loglevel, String topic, String data) {
if (!DebugMqtt::isInit) {
void DebugMqtt::sendData(Loglevel loglevel, String topic, String data)
{
if (!DebugMqtt::isInit)
{
return;
}
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);
if (loglevel <= DebugMqtt::loglevel && loglevel > Loglevel::none)
{
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);
void DebugMqtt::sendData(Loglevel loglevel, String data)
{
DebugMqtt::sendData(loglevel, "", data);
}
void DebugMqtt::writeToInflux(String measurement_name, String field_set, float measurement, uint64_t nanos) {
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;
}
}
void DebugMqtt::init(PubSubClient *client, Loglevel max_loglevel) {
void DebugMqtt::init(PubSubClient *client, Loglevel max_loglevel)
{
DebugMqtt::client = client;
DebugMqtt::loglevel = max_loglevel;
DebugMqtt::isInit = true;
}
void DebugMqtt::changeLoglevel(Loglevel loglevel) {
void DebugMqtt::changeLoglevel(Loglevel loglevel)
{
DebugMqtt::loglevel = loglevel;
}
String DebugMqtt::enum_to_string(Loglevel loglevel) {
String DebugMqtt::enum_to_string(Loglevel loglevel)
{
String topic = "";
topic += MQTT_DEBUG_TOPIC;
switch(loglevel){
case Loglevel::error :
switch (loglevel)
{
case Loglevel::error:
topic += "/Error";
break;
case Loglevel::warn :
case Loglevel::warn:
topic += "/Warn";
break;
case Loglevel::info :
case Loglevel::info:
topic += "/Info";
break;
case Loglevel::debug :
case Loglevel::debug:
topic += "/Debug";
break;
case Loglevel::influx :
case Loglevel::influx:
topic += "/Influx";
break;
default:
@@ -105,5 +121,4 @@ String DebugMqtt::enum_to_string(Loglevel loglevel) {
break;
}
return topic;
}
}
lib/MQTT/debugMqtt.h
+120 -117
View File
@@ -4,9 +4,9 @@
* @brief Inherits a class to send debug messages over MQTT
* @version 0.1
* @date 2021-12-13
*
*
* @copyright Copyright (c) 2021
*
*
*/
#ifndef DEBUG_MQTT_H
#define DEBUG_MQTT_H
@@ -14,10 +14,9 @@
#include <iostream>
#include <PubSubClient.h>
/**
* @brief
*
* @brief
*
* If you want to change the default topic
* to an other value, than you have to define this define
* in your code befor you include this File.
@@ -28,151 +27,155 @@
/**
* @brief Defualt for max message size
*
*
* If you want to change the default size of 128 Byte
* to an other value, than you have to define this define
* in your code befor you include this File.
*/
#ifndef MQTT_BUFFER_SIZE
#define MQTT_BUFFER_SIZE 128
#endif //MQTT_BUFFER_SIZE
#endif // MQTT_BUFFER_SIZE
/**
* @brief An enum to set the log level
*
*
* The log level is the last part of the MQTT topic.
* Unless you give sendMsg() or sendData() a additional
* topic as String.
*
*
* @see sendMsg()
* @see sendData()
*
*
*/
enum Loglevel { none,
error,
warn,
info,
debug,
influx};
enum Loglevel
{
none,
error,
warn,
info,
debug,
influx
};
/**
* @brief A class to send debug messages over MQTT
*
*
* This class send debug messages over MQTT with different topics
* by the enum Loglevel. Besides that this class supports to send
* data via Telegraf into Grafana.
*
*
* @see Loglevel
*/
class DebugMqtt {
public:
/**
* @brief Construct a new Debug Mqtt object.
*
* @param name A String with send with every Message.
* @param bufSize for the addCharacter function.
*/
DebugMqtt(const char* name, uint8_t bufSize = 0);
class DebugMqtt
{
public:
/**
* @brief Construct a new Debug Mqtt object.
*
* @param name A String with send with every Message.
* @param bufSize for the addCharacter function.
*/
DebugMqtt(const char *name, uint8_t bufSize = 0);
~DebugMqtt();
~DebugMqtt();
/**
* @brief Send a Message via MQTT
*
* This function uses sendData to send the given string and
* add the name to the message given by the constructer.
*
* @see sendData()
* @see Loglevel
*
* @param loglevel Loglevel given by the enum Loglevel
* @param topic Additional topic behind loglevel
* @param msg The message to send as String
*/
void sendMsg(Loglevel loglevel, String topic, String msg);
void sendMsg(Loglevel loglevel, String msg);
/**
* @brief Send a Message via MQTT
*
* This function uses sendData to send the given string and
* add the name to the message given by the constructer.
*
* @see sendData()
* @see Loglevel
*
* @param loglevel Loglevel given by the enum Loglevel
* @param topic Additional topic behind loglevel
* @param msg The message to send as String
*/
void sendMsg(Loglevel loglevel, String topic, String msg);
void sendMsg(Loglevel loglevel, String msg);
/**
* @brief Send a Message via MQTT
*
* This function sends the Data via MQTT with the given topic
* from Loglevel or followed by given String topic.
* Normally this function is called by sendMsg() or by
* writeToInflux()
*
* @see sendData()
* @see writeToInflux()
* @see Loglevel
*
* @param loglevel Loglevel given by the enum Loglevel
* @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);
/**
* @brief Send a Message via MQTT
*
* This function sends the Data via MQTT with the given topic
* from Loglevel or followed by given String topic.
* Normally this function is called by sendMsg() or by
* writeToInflux()
*
* @see sendData()
* @see writeToInflux()
* @see Loglevel
*
* @param loglevel Loglevel given by the enum Loglevel
* @param topic Additional topic behind loglevel
* @param data The message to send as String
*/
static void sendData(Loglevel loglevel, String topic, String data);
static void sendData(Loglevel loglevel, String data);
/**
* @brief Send a Message via MQTT for InfluxDB
*
* This function sends a MQTT message which is intended for
* Telegraf. Telegraf can listen on MQTT messages and put
* them in an Influx Database.
*
* @param measurement_name like a category
* @param field_set the name of the value e.g temperature
* @param measurement the real value
* @param nanos Current time in nanoseconds
*/
void writeToInflux(String measurement_name, String field_set, float measurement, uint64_t nanos);
/**
* @brief Send a Message via MQTT for InfluxDB
*
* This function sends a MQTT message which is intended for
* Telegraf. Telegraf can listen on MQTT messages and put
* them in an Influx Database.
*
* @param measurement_name like a category
* @param field_set the name of the value e.g temperature
* @param measurement the real value
* @param nanos Current time in nanoseconds
*/
void writeToInflux(String measurement_name, String field_set, float measurement, uint64_t nanos);
/**
* @brief Adds a single character to the buf
*
* The buf will be flushed out:
* 1. when the buffer is full
* 2. when the character is '\n'
*
* @param c
*/
void addCharacter(char c);
/**
* @brief Adds a single character to the buf
*
* The buf will be flushed out:
* 1. when the buffer is full
* 2. when the character is '\n'
*
* @param character
*/
void addCharacter(char character);
/**
* @brief Initialize debugMQTT for all instances
*
* You only have to call this function once for your project.
* If you call this function again you overwrite the client and
* the loglevel. If you only want du overwrite the max_loglevel
* use changeLoglevel()
*
* @see changeLoglevel()
*
* @param client PubSubClient
* @param max_loglevel Max loglevel to send.
*/
static void init(PubSubClient* client, Loglevel max_loglevel);
/**
* @brief Initialize debugMQTT for all instances
*
* You only have to call this function once for your project.
* If you call this function again you overwrite the client and
* the loglevel. If you only want du overwrite the max_loglevel
* use changeLoglevel()
*
* @see changeLoglevel()
*
* @param client PubSubClient
* @param max_loglevel Max loglevel to send.
*/
static void init(PubSubClient *client, Loglevel max_loglevel);
/**
* @brief Change loglevel
*
* This function changes the maximum loglevel which be send.
*
* @param loglevel
*/
static void changeLoglevel(Loglevel loglevel);
/**
* @brief Change loglevel
*
* This function changes the maximum loglevel which be send.
*
* @param loglevel
*/
static void changeLoglevel(Loglevel loglevel);
private:
const char* name;
char* buf;
uint8_t bufPos = 0;
uint8_t bufSize = 100;
private:
const char *name;
char *buf;
uint8_t bufPos = 0;
uint8_t bufSize = 100;
static String enum_to_string(Loglevel loglevel);
static String enum_to_string(Loglevel loglevel);
static PubSubClient* client;
static Loglevel loglevel;
static bool isInit;
static char msg[MQTT_BUFFER_SIZE];
static char topic[MQTT_BUFFER_SIZE];
static PubSubClient *client;
static Loglevel loglevel;
static bool isInit;
static char msg[MQTT_BUFFER_SIZE];
static char topic[MQTT_BUFFER_SIZE];
};
#endif // DEBUG_MQTT_H
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/Network/network.cpp
+201
View File
@@ -0,0 +1,201 @@
/**
* @file network.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-09-18
*
* @copyright Copyright (c) 2023
*
*/
#include "network.h"
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, NetworkAddresses adresses)
: addresses{adresses}
{
if (!WiFiGenericClass::mode(WIFI_AP_STA))
{
std::cout << "Network::connectWiFi failed WiFi.mode" << std::endl;
}
if (!WiFi.config(this->addresses.localIP,
this->addresses.gateway,
this->addresses.subnet,
this->addresses.dnsServer))
{
std::cout << "STA Failed to configure" << std::endl;
}
this->init(ssid, passphrase);
}
Network::~Network()
{
delete mqttClient;
}
bool Network::activateEspNow(receiveCallbackPtr reci, sendCallbackPtr send)
{
if (esp_now_init() != ESP_OK)
{
std::cout << "Network::activateEspNow - Error initializing ESP-NOW" << std::endl;
return false;
}
esp_now_register_send_cb(send);
esp_now_peer_info_t peerInfo = {};
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)
{
std::cout << "Network::connectEspNow - Failed to add peer" << std::endl;
return false;
}
esp_now_register_recv_cb(reci);
return true;
}
bool Network::activateMqtt(const char *user, const char *passphrase)
{
this->mqttUser = user;
this->mqttPassphrase = passphrase;
this->mqttClient = new PubSubClient(this->wifiClient);
this->mqttClient->setServer(this->addresses.mqttServer, this->addresses.mqttPort);
this->mqttClient->setSocketTimeout(1);
if (this->wifiConnected)
{
return this->connectMqtt();
}
return false;
}
const void Network::printIPs()
{
std::cout << std::endl;
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;
}
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()
{
if (!this->initSuccessful)
{
return;
}
this->checkWifi();
if (this->wifiConnected && static_cast<bool>(this->mqttClient))
{
this->checkMqtt();
}
}
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 (WiFiSTAClass::status() != WL_CONNECTED)
{
delay(Network::wifiConnectLoopTime);
std::cout << "." << std::flush;
timeout--;
if (timeout == 0)
{
std::cout << std::endl;
std::cout << "WiFi NOT connected." << std::endl;
return;
}
}
this->wifiConnected = true;
this->printIPs();
}
void Network::checkWifi()
{
if ((WiFiSTAClass::status() != WL_CONNECTED) && (millis() - this->lastWifiReconnectAttempt >= Network::wifiReconnectDelay))
{
std::cout << "Reconnecting to WiFi..." << std::endl;
WiFi.disconnect();
this->wifiConnected = WiFi.reconnect();
this->lastWifiReconnectAttempt = millis();
}
}
void Network::checkMqtt()
{
if (!this->mqttClient->connected() && millis() - this->lastMqttReconnectAttempt > Network::mqttReconnectDelay)
{
this->mqttConnected = this->connectMqtt();
this->lastMqttReconnectAttempt = millis();
}
if (this->mqttConnected)
{
this->mqttClient->loop();
}
}
bool Network::connectMqtt()
{
String clientId = "ESP32Rover-";
clientId += String(random(), HEX);
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");
}
}
return this->mqttClient->connected();
}
lib/Network/network.h
+136
View File
@@ -0,0 +1,136 @@
/**
* @file network.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-09-18
*
* @copyright Copyright (c) 2023
*
*/
#ifndef NETWORK_H
#define NETWORK_H
#include "component.h"
#include <iostream>
#include <WiFi.h>
#include <PubSubClient.h>
#include <esp_now.h>
#include <esp_wifi.h>
/**
* @brief Typedef to easy handle the receive Callback
*/
typedef void (*receiveCallbackPtr)(const uint8_t *mac, const uint8_t *incomingData, int len);
/**
* @brief Typedef to easy handle the send Callback
*/
typedef void (*sendCallbackPtr)(const uint8_t *mac_addr, esp_now_send_status_t status);
/**
* @brief A Struct to hold all network addresses
*/
struct NetworkAddresses
{
IPAddress localIP;
IPAddress gateway;
IPAddress subnet;
IPAddress dnsServer;
IPAddress mqttServer;
uint16_t mqttPort = 1883;
};
/**
* @brief A class to manage the wireless connections
*/
class Network : public Component
{
public:
/**
* @brief Construct a new Network object
*
* With this constructor the esp gets its ip from a Dhcp server
*
* @param ssid
* @param passphrase
*/
Network(const char *ssid, const char *passphrase);
/**
* @brief Construct a new Network object
*
* This constructor is used for a static ip setup
*
* @param ssid
* @param passphrase
* @param addresses
*/
Network(const char *ssid, const char *passphrase, NetworkAddresses addresses);
~Network();
/**
* @brief Activate ESP-NOW to communicate with the remote Control
*
* @param receive
* @param send
* @return true
* @return false
*/
bool activateEspNow(receiveCallbackPtr receive, sendCallbackPtr send);
/**
* @brief Activate MQTT to send debug messages
*
* @param user
* @param passphrase
* @return true
* @return false
*/
bool activateMqtt(const char *user = nullptr, const char *passphrase = nullptr);
/**
* @brief Print the current used IPs
*/
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; }
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();
NetworkAddresses addresses;
WiFiClient wifiClient;
PubSubClient *mqttClient = nullptr;
bool initSuccessful = false;
bool wifiConnected = false;
bool mqttConnected = false;
const char *mqttUser = nullptr;
const char *mqttPassphrase = nullptr;
uint8_t broadcastAddress[6] = {0xC8, 0xC9, 0xA3, 0xC8, 0x57, 0x10};
uint32_t lastWifiReconnectAttempt = 0;
uint32_t lastMqttReconnectAttempt = 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
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
+109 -107
View File
@@ -1,139 +1,141 @@
/**
* @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
*
* @brief A class to handle points on the earth
*
* The points inherits latitude 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
* @param lon
* @param horizontalAccuracy
* @param creationTime
*/
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/Sensors/sensorData.cpp
+177 -59
View File
@@ -1,107 +1,195 @@
/**
* @file senors.cpp
* @file sensorData.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;
bool SensorData::newData = 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;
}
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::enableGyroscope()
{
this->gyroscope = new MPU6050();
this->gyroscope->initialize();
if (!this->gyroscope->testConnection())
{
std::cout << "SensorData::enableGyroscope: Gyroskop is not conntected. Freeze!" << std::endl;
while (true)
{
}
}
const uint8_t deviceStatus = this->gyroscope->dmpInitialize();
// TODO: !!! MagicNumer 6x
this->gyroscope->setXGyroOffset(220);
this->gyroscope->setYGyroOffset(76);
this->gyroscope->setZGyroOffset(-85);
this->gyroscope->setZAccelOffset(1788);
if (deviceStatus == 0)
{
this->gyroscope->CalibrateAccel(6);
this->gyroscope->CalibrateGyro(6);
this->gyroscope->PrintActiveOffsets();
this->gyroscope->setDMPEnabled(true);
}
else
{
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
std::cout << "SensorData::enableGyroscope: 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;
}
void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
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)
{
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
@@ -112,35 +200,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::newData = true;
SensorData::ubxDataStatic = ubxDataStruct;
SensorData::ubxUpdateTimeStatic = millis();
}
void SensorData::setOutputStatusPrintPVTdata(bool status) {
void SensorData::setOutputStatusPrintPVTdata(bool status)
{
SensorData::outputStatusPrintPVTdata = status;
}
void SensorData::run() {
this->realCompass->read();
this->realAzimuth = this->realCompass->getAzimuth();
void SensorData::run()
{
if (static_cast<bool>(this->realCompass))
{
this->realCompass->read();
this->realAzimuth = this->realCompass->getAzimuth();
}
if (static_cast<bool>(this->gyroscope) && this->gyroscope->dmpGetCurrentFIFOPacket(static_cast<uint8_t *>(this->gyroBuffer)))
{
this->gyroscope->dmpGetQuaternion(&this->quaternion, static_cast<uint8_t *>(this->gyroBuffer));
this->gyroscope->dmpGetGravity(&this->gravity, &this->quaternion);
this->gyroscope->dmpGetYawPitchRoll(static_cast<float *>(this->yawPitchRoll), &this->quaternion, &this->gravity);
}
}
void SensorData::runAsChild() {
this->gnss->checkUblox();
this->gnss->checkCallbacks();
if (SensorData::newData)
SensorData::newData = false;
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);
@@ -150,3 +256,15 @@ void SensorData::initGnss() {
this->gnss->setNavigationFrequency(1);
this->gnss->setAutoPVT(true);
}
void SensorData::updateUbxData()
{
this->gnssData = SensorData::ubxDataStatic;
this->lastUbxUpdate = SensorData::ubxUpdateTimeStatic;
Point::Coordinates coords{0, 0};
coords.lat = this->gnssData->lat / 10000000.0;
coords.lon = this->gnssData->lon / 10000000.0;
this->currentPosition = Point(coords, this->gnssData->hAcc);
}
lib/Sensors/sensorData.h
+142 -52
View File
@@ -1,18 +1,19 @@
/**
* @file sensorData.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief Contains the SensorData class
* @version 0.1
* @date 2023-09-02
*
*
* @copyright Copyright (c) 2023
*
*
*/
#ifndef SENSOR_DATA_H
#define SENSOR_DATA_H
#include <SPI.h>
#include <I2Cdev.h>
#include <iostream>
#include "component.h"
@@ -20,70 +21,159 @@
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include <QMC5883LCompass.h>
// Gyroskop
#include <MPU6050_6Axis_MotionApps20.h>
#include "calcAzimuth.h"
#include "point.h"
class Sensors;
class SensorData : public Component {
public:
SensorData();
void enableGnss(SPIClass* spiPort, uint8_t csPin);
void enableGnss();
void enableRealCompass();
void enableCalcCompass();
void enableGyroskop();
/**
* @brief A class to manage all sensors
*
* Each sensor have separately to be enabled
*/
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);
Point getCurrentPos() const { return this->currentPosition; }
const UBX_NAV_PVT_data_t* getGnssData() const { return this->gnssData; };
const void* const getGyroData() const;
/**
* @brief Enable the gnss module over spi
*
* @param spiPort
* @param csPin
*/
void enableGnss(SPIClass *spiPort, uint8_t csPin);
CalcAzimuth* getCalcCompass() const { return this->calcCompass; }
QMC5883LCompass* getRealCompass() const { return this->realCompass; }
/**
* @brief Enable the gnss module over i2c
*/
void enableGnss();
// 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);
void enableRealCompass();
void enableCalcCompass();
void enableGyroscope();
private:
void run() override;
void runAsChild() override;
void initGnss();
// Interface Const
/**
* @brief Get the azimuth measured by the compass module
*
* @return int16_t
*/
int16_t getRealAzimuth() const { return this->realAzimuth; }
QMC5883LCompass* realCompass = nullptr;
CalcAzimuth* calcCompass = nullptr;
SFE_UBLOX_GNSS* gnss = nullptr;
/**
* @brief Get the azimuth calculated by CalcAzimuth
*
* Consider to call getCalcAzimuthState() to check, if the data is valid.
*
* @return int16_t
*/
int16_t getCalcAzimuth() const { return this->calcAzimuth; }
UBX_NAV_PVT_data_t* gnssData;
Point currentPosition;
/**
* @brief Get the CalcAzimuth::State object
*
* Needed to check the quality of calculated azimuth
*
* @return CalcAzimuth::State
*/
CalcAzimuth::State getCalcAzimuthState() const;
int16_t realAzimuth = INT16_MAX;
int16_t calcAzimuth = INT16_MAX;
Point getCurrentPos() const { return this->currentPosition; }
const UBX_NAV_PVT_data_t *getGnssData() const { return this->gnssData; };
NTRIPClientStates getNtripState() const;
// static
static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
/**
* @brief Get the data from the gyroscope
*
* The returned float pointer is an array of 3 floats
* - Yaw
* - Pitch
* - Roll
*
* @return const float*
*/
const float *getGyroData() const { return this->yawPitchRoll; }
static UBX_NAV_PVT_data_t* ubxDataStatic;
/**
* @brief Get the CalcCompass object
* @return CalcAzimuth*
*/
CalcAzimuth *getCalcCompass() const { return this->calcCompass; }
static uint32_t ubxUpdateTimeStatic;
/**
* @brief Get the RealCompass object
* @return QMC5883LCompass*
*/
QMC5883LCompass *getRealCompass() const { return this->realCompass; }
static bool outputStatusPrintPVTdata;
static bool newData;
/**
* @brief Get the NTRIPClient object
* @return NTRIPClient*
*/
NTRIPClient *getNtripClient() const { return this->ntripClient; }
/**
* @brief Get the Gyroscope object
* @return MPU6050*
*/
MPU6050 *getGyroscope() const { return this->gyroscope; }
// 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 *gyroscope = nullptr;
UBX_NAV_PVT_data_t *gnssData = nullptr;
Point currentPosition;
Quaternion quaternion;
VectorFloat gravity;
char *host = nullptr;
char *mountPoint = nullptr;
char *user = nullptr;
char *password = nullptr;
bool isNtripInit = false;
uint8_t gyroBuffer[64];
uint16_t port = 0;
int16_t realAzimuth = INT16_MAX;
int16_t calcAzimuth = INT16_MAX;
uint32_t lastUbxUpdate = 0;
float yawPitchRoll[3]{0, 0, 0};
// 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
+79 -46
View File
@@ -5,117 +5,150 @@
* @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(1000); // 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();
Component::loopDelay = Speedometer::loopDelay;
clearAvgBuf();
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
const double wheelRevolutionsAbsolute = pulse / this->steps;
const double wheelRevolutionsRelativ = wheelRevolutionsAbsolute / (elapsedTime / 1000.0);
switch (this->currentDirection) {
case Direction::Forward :
this->speed = ms;
break;
case Direction::Backward :
this->speed = -ms;
break;
double meterPerSecond = wheelRevolutionsRelativ * (diameter * PI);
double radPerSecond = wheelRevolutionsRelativ * 2 * PI;
case Direction::None :
this->speed = 0;
break;
if (meterPerSecond < minimalSpeed)
{
meterPerSecond = 0;
radPerSecond = 0;
}
switch (this->currentDirection)
{
case Direction::Forward:
this->speed = meterPerSecond;
this->rad = radPerSecond;
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
+103 -96
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,120 +21,127 @@
/**
* @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 number of last values ​​to be taken into account for the average.
*
* @param val length of the array
*/
void setNumOfValForAvg(uint8_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 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; }
/**
* @brief Get the calculated speed of the Wheel
*
* @return double speed in m/s
*/
double getSpeed() const { return this->speed; }
double getAvgSpeed() const;
/**
* @brief Get the calculated speed of the wheel
*
* @return double speed in rad/s
*/
double getSpeedRad() const { return this->rad; };
/**
* @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 Get the calculated average speed of the wheel
*
* @return double speed in m/s
*/
double getAvgSpeed() const;
/**
* @brief Stop calibration
*
* Start the loop function and read the past steps.
*
* @return uint16_t steps since calibrationMeasurementStart was called
*/
uint16_t calibrationMeasurementStop();
/**
* @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();
private:
void run() override;
void init(uint8_t pin, double diameter, uint16_t steps);
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 diameter;
double speed = 0;
double rad = 0;
double diameter;
uint8_t printCounter = 0;
uint8_t bufPos = 0;
uint16_t steps;
uint8_t printCounter = 0;
uint8_t bufPos = 0;
uint16_t steps;
int16_t buf[Speedometer::bufSize];
uint32_t lastMillisCalc = 0;
int16_t buf[Speedometer::bufSize];
uint32_t lastMillisCalc = 0;
static constexpr uint16_t maxFilterValue = 1023;
};
#endif // SPEEDOMETER_H
lib/Times/debugTimes.cpp
+24 -13
View File
@@ -1,45 +1,56 @@
/**
* @file debugTimes.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief Implemention of the class debugTimes.h.
* @brief Implementation 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 muted, 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.h
-60
View File
@@ -1,60 +0,0 @@
/**
* @file calibrateCompass.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-05-23
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include <QMC5883LCompass.h>
#include <Preferences.h>
#include <iostream>
#include "component.h"
class CalibrateCompass : public Component {
public:
enum State {
Ready,
Calibrating,
Finished
};
struct CallibrationData {
int data[3][2];
};
CalibrateCompass(QMC5883LCompass* compass);
void start();
void useData();
void removeCalibration();
void reset();
void saveData();
void loadData();
State getState() const { return this->state; }
CallibrationData getCallibrationData() const { return this->data; }
friend std::ostream& operator<<(std::ostream& os, const CalibrateCompass& caliComp);
private:
void runAsChild() override;
void run() override;
void checkDataValidity();
QMC5883LCompass* compass;
State state;
CallibrationData data;
void clearData();
bool dataValid = false;
const uint16_t maxTimeWithoutChange = 10000;
uint32_t lastChange = 0;
};