amikhalev/uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

move LatLong to own file

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-15 21:31:06 -07:00
parent b7c3278b6f
commit fe5160abfc
6 changed files with 69 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
main/CMakeLists.txt
View File

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

33
main/lat_long.cc Normal file
View File

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

31
main/lat_long.hh Normal file
View File

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

2
main/pid_controller.cc
View File

@ -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;

2
main/pid_controller.hh
View File

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

57
main/ugv_main.cc
View File

@ -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;