|
|
@ -39,6 +39,8 @@ |
|
|
|
* |
|
|
|
* |
|
|
|
*/ |
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#define __STDC_FORMAT_MACROS |
|
|
|
|
|
|
|
#include <inttypes.h> |
|
|
|
#include <math.h> |
|
|
|
#include <math.h> |
|
|
|
#include "estimator_base.h" |
|
|
|
#include "estimator_base.h" |
|
|
|
#include <mathlib/mathlib.h> |
|
|
|
#include <mathlib/mathlib.h> |
|
|
@ -381,7 +383,7 @@ void EstimatorBase::printStoredIMU() |
|
|
|
|
|
|
|
|
|
|
|
void EstimatorBase::printIMU(struct imuSample *data) |
|
|
|
void EstimatorBase::printIMU(struct imuSample *data) |
|
|
|
{ |
|
|
|
{ |
|
|
|
printf("time %llu\n", data->time_us); |
|
|
|
printf("time %" PRIu64 "\n", data->time_us); |
|
|
|
printf("delta_ang_dt %.5f\n", (double)data->delta_ang_dt); |
|
|
|
printf("delta_ang_dt %.5f\n", (double)data->delta_ang_dt); |
|
|
|
printf("delta_vel_dt %.5f\n", (double)data->delta_vel_dt); |
|
|
|
printf("delta_vel_dt %.5f\n", (double)data->delta_vel_dt); |
|
|
|
printf("dA: %.5f %.5f %.5f \n", (double)data->delta_ang(0), (double)data->delta_ang(1), (double)data->delta_ang(2)); |
|
|
|
printf("dA: %.5f %.5f %.5f \n", (double)data->delta_ang(0), (double)data->delta_ang(1), (double)data->delta_ang(2)); |
|
|
@ -409,14 +411,14 @@ void EstimatorBase::printStoredMag() |
|
|
|
|
|
|
|
|
|
|
|
void EstimatorBase::printMag(struct magSample *data) |
|
|
|
void EstimatorBase::printMag(struct magSample *data) |
|
|
|
{ |
|
|
|
{ |
|
|
|
printf("time %llu\n", data->time_us); |
|
|
|
printf("time %" PRIu64 "\n", data->time_us); |
|
|
|
printf("mag: %.5f %.5f %.5f \n\n", (double)data->mag(0), (double)data->mag(1), (double)data->mag(2)); |
|
|
|
printf("mag: %.5f %.5f %.5f \n\n", (double)data->mag(0), (double)data->mag(1), (double)data->mag(2)); |
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void EstimatorBase::printBaro(struct baroSample *data) |
|
|
|
void EstimatorBase::printBaro(struct baroSample *data) |
|
|
|
{ |
|
|
|
{ |
|
|
|
printf("time %llu\n", data->time_us); |
|
|
|
printf("time %" PRIu64 "\n", data->time_us); |
|
|
|
printf("baro: %.5f\n\n", (double)data->hgt); |
|
|
|
printf("baro: %.5f\n\n", (double)data->hgt); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
@ -431,7 +433,7 @@ void EstimatorBase::printStoredBaro() |
|
|
|
|
|
|
|
|
|
|
|
void EstimatorBase::printGps(struct gpsSample *data) |
|
|
|
void EstimatorBase::printGps(struct gpsSample *data) |
|
|
|
{ |
|
|
|
{ |
|
|
|
printf("time %llu\n", data->time_us); |
|
|
|
printf("time %" PRIu64 "\n", data->time_us); |
|
|
|
printf("gps pos: %.5f %.5f %.5f\n", (double)data->pos(0), (double)data->pos(1), (double)data->hgt); |
|
|
|
printf("gps pos: %.5f %.5f %.5f\n", (double)data->pos(0), (double)data->pos(1), (double)data->hgt); |
|
|
|
printf("gps vel %.5f %.5f %.5f\n\n", (double)data->vel(0), (double)data->vel(1), (double)data->vel(2)); |
|
|
|
printf("gps vel %.5f %.5f %.5f\n\n", (double)data->vel(0), (double)data->vel(1), (double)data->vel(2)); |
|
|
|
} |
|
|
|
} |
|
|
|