Commit 703fa7f8 authored by Léo-Paul Géneau's avatar Léo-Paul Géneau 👾

Stop relying on gps messages

Use position messages instead of gps ones in case of gps denial.
parent 40a276a2
...@@ -43,7 +43,6 @@ static const double COS_ADDED_DISTANCE = cos(ADDED_DISTANCE); ...@@ -43,7 +43,6 @@ static const double COS_ADDED_DISTANCE = cos(ADDED_DISTANCE);
static const double SIN_ADDED_DISTANCE = sin(ADDED_DISTANCE); static const double SIN_ADDED_DISTANCE = sin(ADDED_DISTANCE);
static int mavsdk_started = 0; static int mavsdk_started = 0;
static uint64_t init_timestamp;
static Telemetry::Position origin; static Telemetry::Position origin;
static Coordinates targeted_destination; static Coordinates targeted_destination;
...@@ -169,19 +168,22 @@ static void updateProjection(double current_lat, double current_lon) { ...@@ -169,19 +168,22 @@ static void updateProjection(double current_lat, double current_lon) {
int updateLogAndProjection(void) { int updateLogAndProjection(void) {
std::ostringstream oss; std::ostringstream oss;
Telemetry::RawGps gps; Telemetry::Position position;
Telemetry::FixedwingMetrics metrics;
if(mavsdk_started) { if(mavsdk_started) {
gps = telemetry->raw_gps (); const auto now = std::chrono::system_clock::now();
oss << (gps.timestamp_us - init_timestamp) / 1000 << ";" position = telemetry->position();
<< gps.latitude_deg << ";" << gps.longitude_deg << ";" metrics = telemetry->fixedwing_metrics();
<< gps.absolute_altitude_m << ";" << telemetry->position().relative_altitude_m << ";" oss << std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count() << ";"
<< position.latitude_deg << ";" << position.longitude_deg << ";"
<< position.absolute_altitude_m << ";" << position.relative_altitude_m << ";"
<< telemetry->attitude_euler().yaw_deg << ";" << telemetry->attitude_euler().yaw_deg << ";"
<< gps.velocity_m_s << ";" << telemetry->fixedwing_metrics().climb_rate_m_s; << metrics.velocity_m_s << ";" << metrics.climb_rate_m_s;
log(oss.str()); log(oss.str());
if (projected_destination.latitude != 0 || targeted_destination.latitude != 0) { if (projected_destination.latitude != 0 || targeted_destination.latitude != 0) {
return updateProjection(gps.latitude_deg, gps.longitude_deg); return updateProjection(position.latitude_deg, position.longitude_deg);
} }
} }
...@@ -234,11 +236,10 @@ int start(const char * ip, int port, const char * log_file, int timeout) { ...@@ -234,11 +236,10 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
} while(isnan(abs_altitude) || abs_altitude == 0); } while(isnan(abs_altitude) || abs_altitude == 0);
origin = telemetry->position(); origin = telemetry->position();
init_timestamp = telemetry->raw_gps().timestamp_us;
log("MAVSDK started..."); log("MAVSDK started...");
mavsdk_started = 1; mavsdk_started = 1;
log("timestamp (ms);latitude (°);longitude (°);AMSL (m);rel altitude (m);yaw (°);ground speed (m/s);climb rate (m/s)"); log("timestamp (ms);latitude (°);longitude (°);AMSL (m);rel altitude (m);yaw (°);air speed (m/s);climb rate (m/s)");
return 0; return 0;
} }
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment