Commit 6155425e authored by Léo-Paul Géneau's avatar Léo-Paul Géneau 👾

Subcribe to GLOBAL_POSITION_INT messages

Fetch position from GLOBAL_POSITION_INT to get timestamped positions
parent a6619148
......@@ -44,6 +44,8 @@ static const double SIN_ADDED_DISTANCE = sin(ADDED_DISTANCE);
static int mavsdk_started = 0;
static Telemetry::Position origin;
static int64_t current_position[POSITION_ARRAY_SIZE] = {0};
static int64_t timestamp_difference = 0;
static Coordinates targeted_destination;
static Coordinates projected_destination;
......@@ -161,8 +163,8 @@ static void doReposition_async(float la, float lo, float radius, float y) {
std::thread(doReposition, la, lo, radius, y).detach();
}
static void updateProjection(double current_lat, double current_lon) {
Coordinates position{ current_lat, current_lon };
static void updateProjection(int64_t current_lat, int64_t current_lon) {
Coordinates position{ (double)current_lat / 1e7, (double)current_lon / 1e7 };
projected_destination = getProjectionCoordinates(targeted_destination,
position);
......@@ -181,22 +183,19 @@ static long long int getTimestamp() {
void updateLogAndProjection(void) {
std::ostringstream oss;
Telemetry::Position position;
Telemetry::FixedwingMetrics metrics;
long long int timestamp = getTimestamp();
if(mavsdk_started) {
position = telemetry->position();
metrics = telemetry->fixedwing_metrics();
oss << timestamp << ";"
<< position.latitude_deg << ";" << position.longitude_deg << ";"
<< position.absolute_altitude_m << ";" << position.relative_altitude_m << ";"
oss << current_position[4] << ";"
<< current_position[0] << ";" << current_position[1] << ";"
<< current_position[2] << ";" << current_position[3] << ";"
<< telemetry->attitude_euler().yaw_deg << ";"
<< metrics.airspeed_m_s << ";" << metrics.climb_rate_m_s;
log(oss.str());
if (projected_destination.latitude != 0 || targeted_destination.latitude != 0) {
updateProjection(position.latitude_deg, position.longitude_deg);
updateProjection(current_position[0], current_position[1]);
}
}
}
......@@ -240,6 +239,20 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
action = new Action(msystem);
mavlink_passthrough = new MavlinkPassthrough(msystem);
mavlink_passthrough->subscribe_message_async(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, [](const mavlink_message_t& message) {
mavlink_global_position_int_t position;
mavlink_msg_global_position_int_decode(&message, &position);
if (timestamp_difference == 0)
timestamp_difference = getTimestamp() - position.time_boot_ms;
current_position[0] = position.lat;
current_position[1] = position.lon;
current_position[2] = position.alt;
current_position[3] = position.relative_alt;
current_position[4] = position.time_boot_ms + timestamp_difference;
});
do {
log("Waiting for first telemetry");
sleep_for(seconds(1));
......@@ -252,7 +265,7 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
log("MAVSDK started...");
mavsdk_started = 1;
log("timestamp (ms);latitude (°);longitude (°);AMSL (m);rel altitude (m);yaw (°);air speed (m/s);climb rate (m/s)");
log("timestamp (ms);latitude (°E7);longitude (°E7);AMSL (mm);rel altitude (mm);yaw (°);air speed (m/s);climb rate (m/s)");
return 0;
}
......@@ -374,18 +387,17 @@ void setTargetCoordinates(double la, double lo, float a) {
return;
}
Telemetry::Position position = telemetry->position();
targeted_destination.latitude = la;
targeted_destination.longitude = lo;
updateProjection(position.latitude_deg, position.longitude_deg);
updateProjection(current_position[0], current_position[1]);
setAltitude_async(a);
}
// Information functions
float getAltitude(void) {
return telemetry->position().absolute_altitude_m;
return (float)(current_position[2] / 1000);
}
float getInitialAltitude(void) {
......@@ -400,15 +412,9 @@ double getInitialLongitude(void) {
return origin.longitude_deg;
}
double *getPositionArray(void) {
double timestamp = (double) getTimestamp();
Telemetry::Position position = telemetry->position();
double *positionArray = (double*) malloc(POSITION_ARRAY_SIZE * sizeof(double));
positionArray[0] = position.latitude_deg;
positionArray[1] = position.longitude_deg;
positionArray[2] = (double) position.absolute_altitude_m;
positionArray[3] = (double) position.relative_altitude_m;
positionArray[4] = timestamp;
int64_t *getPositionArray(void) {
int64_t *positionArray = (int64_t*) malloc(POSITION_ARRAY_SIZE * sizeof(int64_t));
memcpy(positionArray, current_position, POSITION_ARRAY_SIZE * sizeof(int64_t));
return positionArray;
}
......
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