99 lines
2.3 KiB
C++
99 lines
2.3 KiB
C++
#pragma once
|
|
|
|
#ifdef PS
|
|
#undef PS
|
|
#endif
|
|
|
|
#ifdef F
|
|
#undef F
|
|
#endif
|
|
|
|
#include <Eigen/Dense>
|
|
|
|
#include "HardwareSerial.h"
|
|
|
|
#include "esp_log.h"
|
|
#include <Adafruit_GPS.h>
|
|
#include <cmath>
|
|
#include <optional>
|
|
|
|
const float TO_RAD = M_PI / 180.0f;
|
|
const float KNOTS_TO_M_SEC = 0.5144444f;
|
|
const float earth_radius = 6371000.0f;
|
|
|
|
struct GPS {
|
|
Adafruit_GPS *gps;
|
|
float origin_lat;
|
|
float origin_long;
|
|
|
|
GPS() {
|
|
|
|
origin_lat = INFINITY;
|
|
origin_long = INFINITY;
|
|
}
|
|
|
|
void begin() {
|
|
this->gps = new Adafruit_GPS(&Serial2);
|
|
this->gps->begin(9600);
|
|
this->gps->sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
|
|
|
|
this->gps->sendCommand(PMTK_API_SET_FIX_CTL_5HZ);
|
|
this->gps->sendCommand(PMTK_SET_NMEA_UPDATE_5HZ);
|
|
}
|
|
|
|
bool gps_avaliable() { return this->gps->fix; }
|
|
|
|
std::optional<Eigen::Vector2f> velocity() {
|
|
if (!this->gps->fix) {
|
|
return std::nullopt;
|
|
}
|
|
float speed = this->gps->speed * KNOTS_TO_M_SEC;
|
|
float angle = this->gps->angle * TO_RAD;
|
|
|
|
return Eigen::Vector2f(cos(M_PI_2 - angle), sin(M_PI_2 - angle)) * speed;
|
|
}
|
|
|
|
std::optional<Eigen::Vector3f>
|
|
waypoint_to_xyz(float latitude, float longitude, float height) {
|
|
if (this->origin_lat == INFINITY || this->origin_long == INFINITY) {
|
|
return std::nullopt;
|
|
}
|
|
|
|
float dLat = (latitude - this->origin_lat) * TO_RAD;
|
|
float dLon = (longitude - this->origin_long) * TO_RAD;
|
|
|
|
float meanLat = (this->origin_lat) * TO_RAD;
|
|
|
|
float y = dLat * earth_radius;
|
|
float x = dLon * earth_radius * std::cos(meanLat);
|
|
float z = this->gps->altitude;
|
|
|
|
return Eigen::Vector3f(x, y, z);
|
|
}
|
|
|
|
std::optional<Eigen::Vector3f> get_coordinates() {
|
|
if (this->gps->fix == false || (this->gps->latitudeDegrees == 0.0 &&
|
|
this->gps->longitudeDegrees == 0.0)) {
|
|
return std::nullopt;
|
|
}
|
|
float latitude = this->gps->latitudeDegrees;
|
|
float longitude = this->gps->longitudeDegrees;
|
|
|
|
return this->waypoint_to_xyz(latitude, longitude, this->gps->altitude);
|
|
}
|
|
|
|
void poll() {
|
|
// char c = this->gps->read();
|
|
// if (this->gps->newNMEAreceived()) {
|
|
// char *line = this->gps->lastNMEA();
|
|
// ESP_LOGI("GPS", "NMEA LINE: %s", line);
|
|
// this->gps->parse(line);
|
|
// }
|
|
}
|
|
};
|
|
|
|
inline SemaphoreHandle_t gps_mutex;
|
|
inline GPS *gps = nullptr;
|
|
|
|
void gps_poll_task(void *_);
|