From fe5160abfc83467a9168ddbc875dbe30a2980b77 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Wed, 15 May 2019 21:31:06 -0700 Subject: [PATCH] move LatLong to own file --- main/CMakeLists.txt | 1 + main/lat_long.cc | 33 ++++++++++++++++++++++++ main/lat_long.hh | 31 +++++++++++++++++++++++ main/pid_controller.cc | 2 ++ main/pid_controller.hh | 2 +- main/ugv_main.cc | 57 +----------------------------------------- 6 files changed, 69 insertions(+), 57 deletions(-) create mode 100644 main/lat_long.cc create mode 100644 main/lat_long.hh diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 3048750..8298137 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -14,6 +14,7 @@ set(COMPONENT_SRCS "MadgwickAHRS.cpp" "e32_driver.cc" "pid_controller.cc" + "lat_long.cc" ) set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT}) set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea" "mbedtls") diff --git a/main/lat_long.cc b/main/lat_long.cc new file mode 100644 index 0000000..e835ab3 --- /dev/null +++ b/main/lat_long.cc @@ -0,0 +1,33 @@ +#include "lat_long.hh" + +float LatLong::distance_to(const LatLong &target) const { + float lat1 = latitude * RAD_PER_DEG; + float lat2 = target.latitude * RAD_PER_DEG; + float long1 = longitude * RAD_PER_DEG; + float long2 = target.longitude * RAD_PER_DEG; + float clat1 = cosf(lat1); + float clat2 = cosf(lat2); + float a = powf(sinf((long2 - long1) / 2.f), 2.f) * clat1 * clat2 + + powf(sinf((lat2 - lat1) / 2.f), 2.f); + float d_over_r = 2 * atan2f(sqrtf(a), sqrtf(1 - a)); + return d_over_r * EARTH_RAD; +} + +float LatLong::bearing_toward(const LatLong &target) const { + float dlong = (target.longitude - longitude) * RAD_PER_DEG; + float sdlong = sinf(dlong); + float cdlong = cosf(dlong); + float lat1 = latitude * RAD_PER_DEG; + float lat2 = target.latitude * RAD_PER_DEG; + float slat1 = sinf(lat1); + float clat1 = cosf(lat1); + float slat2 = sinf(lat2); + float clat2 = cosf(lat2); + float num = sdlong * clat2; + float denom = (clat1 * slat2) - (slat1 * clat2 * cdlong); + float course = atan2f(num, denom); + if (course < 0.0) { + course += 2 * PI; + } + return course / RAD_PER_DEG; +} \ No newline at end of file diff --git a/main/lat_long.hh b/main/lat_long.hh new file mode 100644 index 0000000..2b64d77 --- /dev/null +++ b/main/lat_long.hh @@ -0,0 +1,31 @@ +#pragma once + +#include +#include "ugv_comms.hh" + +struct LatLong { + static constexpr float PI = + 3.1415926535897932384626433832795028841971693993751058209749445923078164062; + + static constexpr float RAD_PER_DEG = PI / 180.f; + // Radius of earth in meters + static constexpr float EARTH_RAD = 6372795.f; + + public: + float latitude; + float longitude; + + inline LatLong() : LatLong(0., 0.) {} + + inline LatLong(double latitude_, double longitude_) + : latitude(latitude_), longitude(longitude_) {} + + inline LatLong(const ugv::comms::messages::TargetLocation &loc) + : latitude(loc.latitude()), longitude(loc.longitude()) {} + + /** + * Return distance from this LatLong to target, in meters + */ + float distance_to(const LatLong &target) const; + float bearing_toward(const LatLong &target) const; +}; \ No newline at end of file diff --git a/main/pid_controller.cc b/main/pid_controller.cc index 442bebb..6b459f1 100644 --- a/main/pid_controller.cc +++ b/main/pid_controller.cc @@ -1,5 +1,7 @@ #include "pid_controller.hh" +using namespace std; + float PIDController::clamp_mag(float x, float max_mag) { if (x > max_mag) return max_mag; diff --git a/main/pid_controller.hh b/main/pid_controller.hh index f27de40..360efcd 100644 --- a/main/pid_controller.hh +++ b/main/pid_controller.hh @@ -1,6 +1,6 @@ #pragma once -#include +#include class PIDController { public: diff --git a/main/ugv_main.cc b/main/ugv_main.cc index 3f98e0a..1d3553e 100644 --- a/main/ugv_main.cc +++ b/main/ugv_main.cc @@ -9,6 +9,7 @@ #include "ugv_display.hh" #include "ugv_io.hh" #include "pid_controller.hh" +#include "lat_long.hh" namespace ugv { @@ -24,12 +25,6 @@ SemaphoreHandle_t i2c_mutex; constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; constexpr float LOOP_PERIOD_S = 1000000.f / static_cast(LOOP_PERIOD_US); -static const float PI = - 3.1415926535897932384626433832795028841971693993751058209749445923078164062; - -static const float RAD_PER_DEG = PI / 180.f; -// Radius of earth in meters -static const float EARTH_RAD = 6372795.f; extern "C" void OnTimeout(void *arg); @@ -41,56 +36,6 @@ void UpdateLocationFromGPS(comms::messages::Location &location, location.set_altitude(gps_data.altitude); } -struct LatLong { - public: - float latitude; - float longitude; - - inline LatLong() : LatLong(0., 0.) {} - - inline LatLong(double latitude_, double longitude_) - : latitude(latitude_), longitude(longitude_) {} - - inline LatLong(const comms::messages::TargetLocation &loc) - : latitude(loc.latitude()), longitude(loc.longitude()) {} - - /** - * Return distance from this LatLong to target, in meters - */ - float distance_to(const LatLong &target) const { - float lat1 = latitude * RAD_PER_DEG; - float lat2 = target.latitude * RAD_PER_DEG; - float long1 = longitude * RAD_PER_DEG; - float long2 = target.longitude * RAD_PER_DEG; - float clat1 = cosf(lat1); - float clat2 = cosf(lat2); - float a = powf(sinf((long2 - long1) / 2.f), 2.f) * clat1 * clat2 + - powf(sinf((lat2 - lat1) / 2.f), 2.f); - float d_over_r = 2 * atan2f(sqrtf(a), sqrtf(1 - a)); - return d_over_r * EARTH_RAD; - } - - float bearing_toward(const LatLong &target) const { - float dlong = (target.longitude - longitude) * RAD_PER_DEG; - float sdlong = sinf(dlong); - float cdlong = cosf(dlong); - float lat1 = latitude * RAD_PER_DEG; - float lat2 = target.latitude * RAD_PER_DEG; - float slat1 = sinf(lat1); - float clat1 = cosf(lat1); - float slat2 = sinf(lat2); - float clat2 = cosf(lat2); - float num = sdlong * clat2; - float denom = (clat1 * slat2) - (slat1 * clat2 * cdlong); - float course = atan2f(num, denom); - if (course < 0.0) { - course += 2 * PI; - } - return course / RAD_PER_DEG; - } -}; - - struct State { public: CommsClass * comms;