move LatLong to own file

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")

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;
+}

main/lat_long.hh

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <cmath>
+#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;
+};

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;

main/pid_controller.hh

@@ -1,6 +1,6 @@
 #pragma once
 
-#include <math.h>
+#include <cmath>
 
 class PIDController {
  public:

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<float>(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;