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

- new implementation for course and distance calculation

Browse Source
This commit is contained in:
kleiax
2024-07-03 09:05:06 +02:00
parent 717daef21e
commit dd304cf787
4 changed files with 54 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/TODO allgemein.txt
+5
View File
@@ -34,5 +34,10 @@ Do later:
- navigation
- autopilot
Have to:
-> check new distance calculation between points
-> check new course calculation between points
-> menu route data und ui disconnecten
lib/Point/point.cpp
+37 -11
View File
@@ -56,14 +56,29 @@ bool Point::operator==(const Point &rhs) const
double Point::distanceTo(const Coordinates &point) const
{
const Coordinates begin = this->coordinates;
const Coordinates end = point;
// Old implementation
// const Coordinates begin = this->coordinates;
// const Coordinates end = point;
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);
// 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);
// return sqrt(dx * dx + dy * dy);
double latStart = this->toRad(this->coordinates.lat);
double lonStart = this->toRad(this->coordinates.lon);
double latEnd = this->toRad(point.lat);
double lonEnd = this->toRad(point.lon);
const double earthRadius = 6371.0; //km
double cartesianX_1 = cos(latStart) * lonStart * earthRadius;
double cartesianY_1 = latStart * earthRadius;
double cartesianX_2 = cos(latEnd) * lonEnd * earthRadius;
double cartesianY_2 = latEnd * earthRadius;
return sqrt(pow(cartesianX_2 - cartesianX_1, 2)
+ pow(cartesianY_2 - cartesianY_1, 2));
}
double Point::distanceTo(const Point &point) const
@@ -73,13 +88,24 @@ double Point::distanceTo(const Point &point) const
int16_t Point::courseTo(const Coordinates &point) const
{
const Coordinates begin = this->coordinates;
const Coordinates end = point;
// const Coordinates begin = this->coordinates;
// const Coordinates end = point;
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);
// 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);
return static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
// return static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
double latStart = this->toRad(this->coordinates.lat);
double lonStart = this->toRad(this->coordinates.lon);
double latEnd = this->toRad(point.lat);
double lonEnd = this->toRad(point.lon);
double y = sin(lonEnd - lonStart) * cos(latEnd);
double x = cos(latStart) * sin(latEnd) - sin(latStart) * cos(latEnd) * cos(lonEnd - lonStart);
double theta = atan2(y, x);
return static_cast<int16_t>(this->toDeg(theta)) % 360;
}
int16_t Point::courseTo(const Point &point) const
lib/Point/point.h
+11
View File
@@ -131,6 +131,17 @@ public:
private:
void init(uint32_t horizontalAccuracy, uint32_t creationTime);
double toRad (double degree) const
{
constexpr double radFactor = M_PI / 180.0;
return degree * radFactor;
};
double toDeg(double radiant) const
{
constexpr double degFactor = 180.0 / M_PI;
return radiant * degFactor;
};
Accuracy accuracy = Accuracy::none;
Coordinates coordinates{0, 0};
lib/Route/route.cpp
+1
View File
@@ -88,6 +88,7 @@ Point Route::getPreviousPoint()
this->currentPoint--;
return *this->it;
}
return point;
}