Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Added a lot of logs, changed the way or raw-logging and cleaned test_libeknav_4

Browse Source
This commit is contained in:
Martin Dieblich
2010-10-06 17:28:28 +00:00
parent 185b98a492
commit 338f2e3740
9 changed files with 235 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/Poine/test_libeknav.xml
+1 -1
View File
@@ -4,7 +4,7 @@
<makefile> <makefile>
HOST=auto1 HOST=auto1
USER=root
PAPARAZZI_INC = -I$(PAPARAZZI_HOME)/var/$(AIRCRAFT) \ PAPARAZZI_INC = -I$(PAPARAZZI_HOME)/var/$(AIRCRAFT) \
-I$(PAPARAZZI_SRC)/sw/airborne \ -I$(PAPARAZZI_SRC)/sw/airborne \
-I$(PAPARAZZI_SRC)/sw/include -I$(PAPARAZZI_SRC)/sw/include
doc/pprz_geodetic/ned_enu.tex
+1 -1
View File
@@ -30,7 +30,7 @@ p_{ENU} \qquad p_{ENU} = \begin{pmatrix}
or directly switching the values or directly switching the values
\begin{equation} \begin{equation}
\begin{pmatrix}x \\ y \\ z \end{pmatrix}_{NED} = \begin{pmatrix}x \\ y \\ z \end{pmatrix}_{NED} =
\begin{pmatrix}y \\ x \\ -z \end{pmatrix}_{NED} \begin{pmatrix}y \\ x \\ -z \end{pmatrix}_{ENU}
\end{equation} \end{equation}
\inHfile{ENU\_OF\_TO\_NED(po, pi)}{pprz\_geodetic} \inHfile{ENU\_OF\_TO\_NED(po, pi)}{pprz\_geodetic}
\inHfile{INT32\_VECT2\_ENU\_OF\_NED(o, i)}{pprz\_geodetic\_int} \inHfile{INT32\_VECT2\_ENU\_OF\_NED(o, i)}{pprz\_geodetic\_int}
sw/airborne/fms/libeknav/Makefile
+1 -1
View File
@@ -4,9 +4,9 @@ raw_log_to_ascii: raw_log_to_ascii.c
fetch_log: fetch_log:
scp @auto1:/tmp/log_ins_test3.data .
scp @auto1:/tmp/log_test3.bin . scp @auto1:/tmp/log_test3.bin .
./raw_log_to_ascii > bla.dat ./raw_log_to_ascii > bla.dat
scp @auto1:/tmp/log_ins_test3.data .
clean: clean:
-rm -f *.o *~ *.d -rm -f *.o *~ *.d
sw/airborne/fms/libeknav/ins_qkf.hpp
+1
View File
@@ -164,6 +164,7 @@ struct basic_ins_qkf
Quaterniond initial_orientation = Quaterniond::Identity(), Quaterniond initial_orientation = Quaterniond::Identity(),
const Vector3d& vel_estimate = Vector3d::Zero()); const Vector3d& vel_estimate = Vector3d::Zero());
/* //Old one without orientation_init() /* //Old one without orientation_init()
basic_ins_qkf(const Vector3d& pos_estimate, basic_ins_qkf(const Vector3d& pos_estimate,
double pos_error, double bias_error, double v_error, double pos_error, double bias_error, double v_error,
sw/airborne/fms/libeknav/ins_qkf_predict.cpp
+15 -3
View File
@@ -28,10 +28,9 @@
#include "ins_qkf.hpp" #include "ins_qkf.hpp"
#include "assertions.hpp" #include "assertions.hpp"
#ifdef TIME_OPS #ifdef TIME_OPS
#include "timer.hpp" //#include "timer.hpp"
# include <iostream> #include <iostream>
#endif #endif
using namespace Eigen; using namespace Eigen;
@@ -59,12 +58,25 @@ linear_predict(basic_ins_qkf& _this, const Vector3d& gyro_meas, const Vector3d&
// accel_cov is the relationship between error vectors in the tangent space // accel_cov is the relationship between error vectors in the tangent space
// of the vehicle orientation and the translational reference frame. // of the vehicle orientation and the translational reference frame.
Matrix3d rot = _this.avg_state.orientation.conjugate().toRotationMatrix(); Matrix3d rot = _this.avg_state.orientation.conjugate().toRotationMatrix();
Vector3d accel_ecef = rot*accel_meas; // a_e = (q_e2b)^* x a_b = q_b2e x a_b Vector3d accel_ecef = rot*accel_meas; // a_e = (q_e2b)^* x a_b = q_b2e x a_b
Vector3d accel_gravity = _this.avg_state.position.normalized()*(-9.81); Vector3d accel_gravity = _this.avg_state.position.normalized()*(-9.81);
Vector3d accel_resid = accel_ecef + accel_gravity; // a = (xdd-g)+g = xdd; Vector3d accel_resid = accel_ecef + accel_gravity; // a = (xdd-g)+g = xdd;
#if 0
printf("==================================================\n");
printf("Quaternion: % 1.4f % 1.4f % 1.4f % 1.4f\n",
_this.avg_state.orientation.w(),
_this.avg_state.orientation.x(),
_this.avg_state.orientation.y(),
_this.avg_state.orientation.z());
printf("Accel_meas: % 1.4f % 1.4f % 1.4f\n", accel_meas(0), accel_meas(1), accel_meas(2));
printf("Accel_ecef: % 1.4f % 1.4f % 1.4f\n", accel_ecef(0), accel_ecef(1), accel_ecef(2));
printf("Accel_grav: % 1.4f % 1.4f % 1.4f\n", accel_gravity(0), accel_gravity(1), accel_gravity(2));
#endif
#if 0 #if 0
// This form works well with zero static acceleration. // This form works well with zero static acceleration.
Matrix<double, 3, 3> accel_cov = Matrix<double, 3, 3> accel_cov =
sw/airborne/fms/libeknav/raw_log.h
+2 -1
View File
@@ -3,13 +3,14 @@
#include "math/pprz_algebra_float.h" #include "math/pprz_algebra_float.h"
struct raw_log_entry { struct __attribute__ ((packed)) raw_log_entry{
float time; float time;
struct FloatRates gyro; struct FloatRates gyro;
struct FloatVect3 accel; struct FloatVect3 accel;
struct FloatVect3 mag; struct FloatVect3 mag;
struct FloatVect3 ecef_pos; struct FloatVect3 ecef_pos;
struct FloatVect3 ecef_vel; struct FloatVect3 ecef_vel;
uint8_t data_valid;
}; };
#endif /* LIBEKNAV_RAW_LOG_H */ #endif /* LIBEKNAV_RAW_LOG_H */
sw/airborne/fms/libeknav/raw_log_to_ascii.c
+1
View File
@@ -47,6 +47,7 @@ int main(int argc, char** argv) {
void print_raw_log_entry(struct raw_log_entry* entry){ void print_raw_log_entry(struct raw_log_entry* entry){
printf("%f\t", entry->time); printf("%f\t", entry->time);
printf("%d\t", entry->data_valid);
printf("%+f %+f %+f\t", entry->gyro.p, entry->gyro.q, entry->gyro.r); printf("%+f %+f %+f\t", entry->gyro.p, entry->gyro.q, entry->gyro.r);
printf("%+f %+f %+f\t", entry->accel.x, entry->accel.y, entry->accel.z); printf("%+f %+f %+f\t", entry->accel.x, entry->accel.y, entry->accel.z);
printf("%+f %+f %+f\t", entry->mag.x, entry->mag.y, entry->mag.z); printf("%+f %+f %+f\t", entry->mag.x, entry->mag.y, entry->mag.z);
sw/airborne/fms/libeknav/test_libeknav_4.cpp
+96 -84
View File
@@ -8,12 +8,13 @@ struct timespec start, prev;
FILE* ins_logfile; // note: initilaized in init_ins_state FILE* ins_logfile; // note: initilaized in init_ins_state
unsigned int counter; unsigned int counter;
#if RUN_FILTER
//useless initialization (I hate C++) //useless initialization (I hate C++)
static basic_ins_qkf ins = basic_ins_qkf(Vector3d::Zero(), 0, 0, 0, static basic_ins_qkf ins = basic_ins_qkf(Vector3d::Zero(), 0, 0, 0,
Vector3d::Zero(), Vector3d::Zero(), Vector3d::Zero()); Vector3d::Zero(), Vector3d::Zero(), Vector3d::Zero());
// import most common Eigen types // import most common Eigen types
#endif
USING_PART_OF_NAMESPACE_EIGEN USING_PART_OF_NAMESPACE_EIGEN
int main(int, char *[]) { int main(int, char *[]) {
@@ -40,12 +41,20 @@ int main(int, char *[]) {
static void main_init(void) { static void main_init(void) {
printf("Filter output will be in "); #if RUN_FILTER
printf("FILTER output will be in ");
#if FILTER_OUTPUT_IN_NED #if FILTER_OUTPUT_IN_NED
printf("NED\n"); printf("NED\n");
#else #else
printf("ECEF\n"); printf("ECEF\n");
#endif #endif
#else
printf("Filter wont run\n");
#endif
#if UPDATE_WITH_GRAVITY
printf("the orientation becomes UPDATED with the GRAVITY\n");
#endif
TRACE(TRACE_DEBUG, "%s", "Starting initialization\n"); TRACE(TRACE_DEBUG, "%s", "Starting initialization\n");
@@ -63,9 +72,10 @@ static void main_init(void) {
TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
return; return;
} }
#if RUN_FILTER
init_ins_state(); init_ins_state();
set_reference_direction(); set_reference_direction();
#endif
main_rawlog_init(IMU_LOG_FILE); main_rawlog_init(IMU_LOG_FILE);
@@ -80,44 +90,52 @@ static void main_periodic(int my_sig_num __attribute__ ((unused))) {
} }
uint8_t data_valid = main_dialog_with_io_proc(); uint8_t data_valid = main_dialog_with_io_proc();
#if RUN_FILTER
main_run_ins(data_valid); main_run_ins(data_valid);
main_rawlog_dump(); #endif
main_rawlog_dump(data_valid);
} }
static uint8_t main_dialog_with_io_proc() { static uint8_t main_dialog_with_io_proc() {
struct AutopilotMessageCRCFrame msg_in; DEFINE_AutopilotMessageCRCFrame_IN_and_OUT(message);
struct AutopilotMessageCRCFrame msg_out;
uint8_t crc_valid; uint8_t crc_valid;
// for (uint8_t i=0; i<6; i++) msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i]; // for (uint8_t i=0; i<6; i++) msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i];
spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid); spi_link_send(&message_out, sizeof(struct AutopilotMessageCRCFrame), &message_in, &crc_valid);
struct AutopilotMessageVIUp *in = &msg_in.payload.msg_up; struct AutopilotMessageVIUp *in = &message_in.payload.msg_up;
if(in->valid_sensors & (1<< VI_IMU_DATA_VALID)){ if(IMU_READY(in->valid_sensors)){
RATES_FLOAT_OF_BFP(imu_float.gyro, in->gyro); COPY_RATES_ACCEL_TO_IMU_FLOAT(in);
ACCELS_FLOAT_OF_BFP(imu_float.accel, in->accel);
} }
if(in->valid_sensors & (1<< VI_MAG_DATA_VALID)){ if(MAG_READY(in->valid_sensors)){
MAGS_FLOAT_OF_BFP(imu_float.mag, in->mag); #if SYNTHETIC_MAG_MODE
printf("MAG: %f %f %f\n", imu_float.mag.x, imu_float.mag.y, imu_float.mag.z); EARTHS_GEOMAGNETIC_FIELD_NORMED(imu_float.mag);
#else
COPY_MAG_TO_IMU_FLOAT(in);
#endif
#if PRINT_MAG
printmag();
#endif
} }
if(in->valid_sensors & (1<<VI_GPS_DATA_VALID)){ if(GPS_READY(in->valid_sensors)){
VECT3_COPY(imu_ecef_pos, in->ecef_pos); COPY_GPS_TO_IMU(in);
printf("GPS: %d %d %d\n", imu_ecef_pos.x, imu_ecef_pos.y, imu_ecef_pos.z); #if PRINT_GPS
VECT3_COPY(imu_ecef_vel, in->ecef_vel); printgps();
#endif
} }
return in->valid_sensors; return in->valid_sensors;
} }
#if RUN_FILTER
static void main_run_ins(uint8_t data_valid) { static void main_run_ins(uint8_t data_valid) {
struct timespec now; struct timespec now;
@@ -125,30 +143,28 @@ static void main_run_ins(uint8_t data_valid) {
double dt_imu_freq = 0.001953125; // 1/512; // doesn't work? double dt_imu_freq = 0.001953125; // 1/512; // doesn't work?
ins.predict(RATES_AS_VECTOR(imu_float.gyro), COORDS_AS_VECTOR(imu_float.accel), dt_imu_freq); ins.predict(RATES_AS_VECTOR3D(imu_float.gyro), VECT3_AS_VECTOR3D(imu_float.accel), dt_imu_freq);
if(data_valid & (1<<VI_MAG_DATA_VALID)){ if(MAG_READY(data_valid)){
//ins.obs_vector(reference_direction, COORDS_AS_VECTOR(imu_float.mag), mag_noise); ins.obs_vector(reference_direction, VECT3_AS_VECTOR3D(imu_float.mag), mag_noise);
} }
#if UPDATE_WITH_GRAVITY #if UPDATE_WITH_GRAVITY
if(ABS(FLOAT_VECT3_NORM(imu_float.accel)-9.81)<0.03){ if(CLOSE_TO_GRAVITY(imu_float.accel)){
// use the gravity as reference // use the gravity as reference
ins.obs_vector(ins.avg_state.position.normalized(), COORDS_AS_VECTOR(imu_float.accel), 0.027); ins.obs_vector(ins.avg_state.position.normalized(), VECT3_AS_VECTOR3D(imu_float.accel), 1.0392e-3);
} }
#endif #endif
if(data_valid & (1<<VI_GPS_DATA_VALID)){ if(GPS_READY(data_valid)){
const Vector3d gps_pos_noise = Vector3d::Ones() *10*10; ins.obs_gps_pv_report(VECT3_AS_VECTOR3D(imu_ecef_pos)/100, VECT3_AS_VECTOR3D(imu_ecef_vel)/100, gps_pos_noise, gps_speed_noise);
const Vector3d gps_speed_noise = Vector3d::Ones()*0.1*0.1;
//ins.obs_gps_pv_report(COORDS_AS_VECTOR(imu_ecef_pos)/100, COORDS_AS_VECTOR(imu_ecef_vel)/100, gps_pos_noise, gps_speed_noise);
} }
print_estimator_state(absTime(time_diff(now, start))); print_estimator_state(absTime(time_diff(now, start)));
} }
#endif
@@ -172,20 +188,16 @@ static void on_foo_event(int fd __attribute__((unused)), short event __attribute
} }
#if RUN_FILTER
static void init_ins_state(void){ static void init_ins_state(void){
ins_logfile = fopen(INS_LOG_FILE, "w"); ins_logfile = fopen(INS_LOG_FILE, "w");
LLA_ASSIGN(pos_0_lla, TOULOUSE_LATTITUDE, TOULOUSE_LONGITUDE, TOULOUSE_HEIGHT) LLA_ASSIGN(pos_0_lla, TOULOUSE_LATTITUDE, TOULOUSE_LONGITUDE, TOULOUSE_HEIGHT)
struct EcefCoor_f pos_0_ecef_pprz; PPRZ_LLA_TO_EIGEN_ECEF(pos_0_lla, pos_0_ecef);
ecef_of_lla_f(&pos_0_ecef_pprz, &pos_0_lla);
pos_0_ecef = COORDS_AS_VECTOR(pos_0_ecef_pprz);
printf("Starting position\t%f\t%f\t%f\n", pos_0_ecef(0), pos_0_ecef(1), pos_0_ecef(2)); printf("Starting position\t%f\t%f\t%f\n", pos_0_ecef(0), pos_0_ecef(1), pos_0_ecef(2));
speed_0_ecef = Vector3d::Zero();
ins.avg_state.position = pos_0_ecef; ins.avg_state.position = pos_0_ecef;
ins.avg_state.gyro_bias = Vector3d::Zero(); ins.avg_state.gyro_bias = Vector3d::Zero();
ins.avg_state.orientation = Quaterniond::Identity(); ins.avg_state.orientation = Quaterniond::Identity();
@@ -193,51 +205,50 @@ static void init_ins_state(void){
Matrix<double, 12, 1> diag_cov; Matrix<double, 12, 1> diag_cov;
diag_cov << Vector3d::Ones()*bias_cov_0*bias_cov_0, diag_cov << Vector3d::Ones() * bias_cov_0 * bias_cov_0 ,
Vector3d::Ones()*M_PI*M_PI*0.5, Vector3d::Ones() * M_PI*0.5 * M_PI*0.5 ,
Vector3d::Ones()*pos_cov_0*pos_cov_0, Vector3d::Ones() * pos_cov_0 * pos_cov_0 ,
Vector3d::Ones()*speed_cov_0*speed_cov_0; Vector3d::Ones() * speed_cov_0 * speed_cov_0;
ins.cov = diag_cov.asDiagonal(); ins.cov = diag_cov.asDiagonal();
} }
static void set_reference_direction(void){ static void set_reference_direction(void){
struct DoubleVect3 ref_dir_ned, struct NedCoor_d ref_dir_ned;
struct EcefCoor_d pos_0_ecef_pprz,
ref_dir_ecef; ref_dir_ecef;
EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned); // the "true" magnetic field EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned);
//LAB_REFERENCE(ref_dir_ned); // measured in the LAB
struct DoubleRMat ned2ecef; struct LtpDef_d current_ltp;
/* copied and modified form pprz_geodetic */ VECTOR_AS_VECT3(pos_0_ecef_pprz, pos_0_ecef);
const double sin_lat = sin(pos_0_lla.lat); ltp_def_from_ecef_d(&current_ltp, &pos_0_ecef_pprz);
const double cos_lat = cos(pos_0_lla.lat); ecef_of_ned_vect_d(&ref_dir_ecef, &current_ltp, &ref_dir_ned);
const double sin_lon = sin(pos_0_lla.lon);
const double cos_lon = cos(pos_0_lla.lon);
ned2ecef.m[0] = -sin_lat*cos_lon;
ned2ecef.m[1] = -sin_lon;
ned2ecef.m[2] = -cos_lat*cos_lon;
ned2ecef.m[3] = sin_lat*sin_lon;
ned2ecef.m[4] = cos_lon;
ned2ecef.m[5] = -cos_lat*sin_lon;
ned2ecef.m[6] = cos_lat;
ned2ecef.m[7] = 0.;
ned2ecef.m[8] = -sin_lat;
RMAT_VECT3_MUL(ref_dir_ecef, ned2ecef, ref_dir_ned); // old transformation:
//MAT33_VECT3_TRANSP_MUL(ref_dir_ecef, ned2ecef, ref_dir_ned); //struct DoubleRMat ned2ecef;
reference_direction = COORDS_AS_VECTOR(ref_dir_ecef); //NED_TO_ECEF_MAT(pos_0_lla, ned2ecef.m);
//RMAT_VECT3_MUL(ref_dir_ecef, ned2ecef, ref_dir_ned);
reference_direction = VECT3_AS_VECTOR3D(ref_dir_ecef);
//reference_direction = Vector3d(1, 0, 0); //reference_direction = Vector3d(1, 0, 0);
std::cout <<"reference direction: " << reference_direction.transpose() << std::endl; std::cout <<"reference direction NED : " << VECT3_AS_VECTOR3D(ref_dir_ned).transpose() << std::endl;
std::cout <<"reference direction ECEF: " << reference_direction.transpose() << std::endl;
}
#endif
/* helpstuff */
/** tiny little functions **/
void printmag(void){
printf("MAG: %f %f %f\n", imu_float.mag.x, imu_float.mag.y, imu_float.mag.z);
}
void printgps(void){
printf("GPS: %d %d %d\n", imu_ecef_pos.x, imu_ecef_pos.y, imu_ecef_pos.z);
} }
/** time measurement **/
/* helpstuff
*
*
*
*/
/* time measurement */
double absTime(struct timespec T){ double absTime(struct timespec T){
return (double)(T.tv_sec + T.tv_nsec*1e-9); return (double)(T.tv_sec + T.tv_nsec*1e-9);
@@ -251,11 +262,7 @@ struct timespec time_diff(struct timespec end, struct timespec start){
return dT; return dT;
} }
/* Logging /** Logging **/
*
*
*
*/
static void main_rawlog_init(const char* filename) { static void main_rawlog_init(const char* filename) {
@@ -266,7 +273,7 @@ static void main_rawlog_init(const char* filename) {
} }
} }
static void main_rawlog_dump(void) { static void main_rawlog_dump(uint8_t data_valid) {
struct timespec now; struct timespec now;
clock_gettime(TIMER, &now); clock_gettime(TIMER, &now);
struct raw_log_entry e; struct raw_log_entry e;
@@ -277,10 +284,12 @@ static void main_rawlog_dump(void) {
VECT3_COPY(e.mag, imu_float.mag); VECT3_COPY(e.mag, imu_float.mag);
VECT3_COPY(e.ecef_pos, imu_ecef_pos); VECT3_COPY(e.ecef_pos, imu_ecef_pos);
VECT3_COPY(e.ecef_vel, imu_ecef_vel); VECT3_COPY(e.ecef_vel, imu_ecef_vel);
e.data_valid = data_valid;
write(raw_log_fd, &e, sizeof(e)); write(raw_log_fd, &e, sizeof(e));
} }
#if RUN_FILTER
static void print_estimator_state(double time) { static void print_estimator_state(double time) {
#if FILTER_OUTPUT_IN_NED #if FILTER_OUTPUT_IN_NED
@@ -298,9 +307,9 @@ static void print_estimator_state(double time) {
q_ned2enu, q_ned2enu,
q_ned2body; q_ned2body;
VECTOR_AS_COORDS(pos_ecef,pos_0_ecef); VECTOR_AS_VECT3(pos_ecef,pos_0_ecef);
VECTOR_AS_COORDS(cur_pos_ecef,ins.avg_state.position); VECTOR_AS_VECT3(cur_pos_ecef,ins.avg_state.position);
VECTOR_AS_COORDS(cur_vel_ecef,ins.avg_state.velocity); VECTOR_AS_VECT3(cur_vel_ecef,ins.avg_state.velocity);
ltp_def_from_ecef_d(&current_ltp, &pos_ecef); ltp_def_from_ecef_d(&current_ltp, &pos_ecef);
@@ -321,8 +330,8 @@ static void print_estimator_state(double time) {
fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z); fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z);
QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(), QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), -ins.avg_state.orientation.x(),
ins.avg_state.orientation.y(), ins.avg_state.orientation.z()); -ins.avg_state.orientation.y(), -ins.avg_state.orientation.z());
QUAT_ASSIGN(q_ned2enu, 0, M_SQRT1_2, M_SQRT1_2, 0); QUAT_ASSIGN(q_ned2enu, 0, M_SQRT1_2, M_SQRT1_2, 0);
FLOAT_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef); FLOAT_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
@@ -333,6 +342,9 @@ static void print_estimator_state(double time) {
struct FloatEulers e; struct FloatEulers e;
FLOAT_EULERS_OF_QUAT(e, q_ned2body); FLOAT_EULERS_OF_QUAT(e, q_ned2body);
#if PRINT_EULER_NED
printf("EULER % 6.1f % 6.1f % 6.1f\n", e.phi*180*M_1_PI, e.theta*180*M_1_PI, e.psi*180*M_1_PI);
#endif
fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e.phi, e.theta, e.psi); fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e.phi, e.theta, e.psi);
fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID, fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID,
sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)), sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)),
@@ -355,13 +367,13 @@ static void print_estimator_state(double time) {
fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z); fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z);
struct FloatQuat q; struct FloatQuat q_ecef2body;
QUAT_ASSIGN(q, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(), QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(),
ins.avg_state.orientation.y(), ins.avg_state.orientation.z()); ins.avg_state.orientation.y(), ins.avg_state.orientation.z());
struct FloatEulers e; struct FloatEulers e_ecef2body;
FLOAT_EULERS_OF_QUAT(e, q); FLOAT_EULERS_OF_QUAT(e_ecef2body, q_ecef2body);
fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e.phi, e.theta, e.psi); fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e_ecef2body.phi, e_ecef2body.theta, e_ecef2body.psi);
fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID, fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID,
sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)), sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)),
sqrt(ins.cov( 3, 3)), sqrt(ins.cov( 4, 4)), sqrt(ins.cov( 5, 5)), sqrt(ins.cov( 3, 3)), sqrt(ins.cov( 4, 4)), sqrt(ins.cov( 5, 5)),
@@ -370,4 +382,4 @@ static void print_estimator_state(double time) {
fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2)); fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2));
#endif #endif
} }
#endif
sw/airborne/fms/libeknav/test_libeknav_4.hpp
+99 -57
View File
@@ -4,6 +4,7 @@
#include <Eigen/Core> #include <Eigen/Core>
#include "ins_qkf.hpp" #include "ins_qkf.hpp"
#include "paparazzi_eigen_conversion.h"
#include <stdint.h> #include <stdint.h>
#include <event.h> #include <event.h>
@@ -32,39 +33,34 @@ extern "C" {
#include "math/pprz_geodetic_double.c" #include "math/pprz_geodetic_double.c"
#include <math.h> #include <math.h>
#define FILTER_OUTPUT_IN_NED 1
/* constants */
/** Compilation-control **/
#define RUN_FILTER 1
#define UPDATE_WITH_GRAVITY 0 #define UPDATE_WITH_GRAVITY 0
#define SYNTHETIC_MAG_MODE 1
#define FILTER_OUTPUT_IN_NED 1
#define PRINT_MAG 0
#define PRINT_GPS 1
#define PRINT_EULER_NED 0
/* /** geodetic **/
*
* Initialization
*
*/
/* initial state */
//Toulouse Lat: 43° 35' 24'' Lon: 1° 25' 48'' //Toulouse Lat: 43° 35' 24'' Lon: 1° 25' 48''
#define TOULOUSE_LATTITUDE UGLY_ANGLE_IN_RADIANS(43,35,24) #define TOULOUSE_LATTITUDE ARCSEC_ACRMIN_ANGLE_IN_RADIANS(43,35,24)
#define TOULOUSE_LONGITUDE UGLY_ANGLE_IN_RADIANS(1,25,48) #define TOULOUSE_LONGITUDE ARCSEC_ACRMIN_ANGLE_IN_RADIANS(1,25,48)
#define TOULOUSE_HEIGHT 0 #define TOULOUSE_HEIGHT 0
//Toulouse Declination is 22' West and Inclination 59° 8' Down //Toulouse Declination is 22' West and Inclination 59° 8' Down
#define TOULOUSE_DECLINATION -UGLY_ANGLE_IN_RADIANS(0,22,0) #define TOULOUSE_DECLINATION -ARCSEC_ACRMIN_ANGLE_IN_RADIANS(0,22,0)
#define TOULOUSE_INCLINATION -UGLY_ANGLE_IN_RADIANS(59,8,0) #define TOULOUSE_INCLINATION -ARCSEC_ACRMIN_ANGLE_IN_RADIANS(59,8,0)
struct LlaCoor_f pos_0_lla; /** magnetic field
Vector3d pos_0_ecef; ** how to compute the magnetic field:
Vector3d speed_0_ecef; ** http://gsc.nrcan.gc.ca/geomag/field/comp_e.php
// Vector3d orientation(0., 0., 0.); **
Vector3d bias_0(0., 0., 0.); ** online-calculator:
** http://geomag.nrcan.gc.ca/apps/mfcal-eng.php
/** **/
* how to compute the magnetic field:
* http://gsc.nrcan.gc.ca/geomag/field/comp_e.php
*
* online-calculator:
* http://geomag.nrcan.gc.ca/apps/mfcal-eng.php
*/
#if 0 #if 0
#define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) { \ #define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) { \
ref.z = sin(TOULOUSE_INCLINATION); \ ref.z = sin(TOULOUSE_INCLINATION); \
@@ -73,65 +69,111 @@ Vector3d bias_0(0., 0., 0.);
ref.y = h*sin(TOULOUSE_DECLINATION); \ ref.y = h*sin(TOULOUSE_DECLINATION); \
} }
#else #else
#define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) VECT3_ASSIGN(ref, 0.51292422348174, -0.00331095113378, 0.85842750338526) //#define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) VECT3_ASSIGN(ref, 0.51292422348174, -0.00331095113378, 0.85842750338526)
#define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) VECT3_ASSIGN(ref, 0.51562740288882, -0.05707735220832, 0.85490967783446)
#endif
Vector3d reference_direction;
#if RUN_FILTER
static void set_reference_direction(void);
#endif #endif
// mean of the measurment data /** other **/
#define LAB_REFERENCE(ref) VECT3_ASSIGN(ref, -0.22496030821134, 0.70578892222179, 0.67175505729281) #define GRAVITY 9.81
#define MAX_DISTANCE_FROM_GRAVITY_FOR_UPDATE 0.03
Vector3d reference_direction; /* Initialisation */
static void main_init(void);
/* initial covariance */ /** initial state **/
struct LlaCoor_f pos_0_lla;
Vector3d pos_0_ecef;
Vector3d speed_0_ecef = Vector3d::Zero();
Vector3d bias_0(0., 0., 0.);
#if RUN_FILTER
static void init_ins_state(void);
#endif
/** initial covariance **/
const double pos_cov_0 = 1e4; const double pos_cov_0 = 1e4;
const double speed_cov_0 = 3.; const double speed_cov_0 = 3.;
// const double orientation_cov_0 = RadOfDeg(5.)*RadOfDeg(5.); // const double orientation_cov_0 = RadOfDeg(5.)*RadOfDeg(5.);
const double bias_cov_0 = 0.447; const double bias_cov_0 = 0.447;
const double mag_noise = std::pow(5 / 180.0 * M_PI, 2); const double mag_noise = std::pow(5 / 180.0 * M_PI, 2);
/* system noise */ /* system noise */
const double mag_error = 2.536e-3; const double mag_error = 2.536e-3;
const Vector3d gyro_white_noise(1.1328*1.1328e-4, 0.9192*0.9192e-4, 1.2291*1.2291e-4); const Vector3d gyro_white_noise ( 1.1328*1.1328e-4, 0.9192*0.9192e-4, 1.2291*1.2291e-4);
const Vector3d gyro_stability_noise(-1.7605*1.7605e-4, 0.5592*0.5592e-4, 1.1486*1.1486e-4 ); const Vector3d gyro_stability_noise ( -1.7605*1.7605e-4, 0.5592*0.5592e-4, 1.1486*1.1486e-4);
const Vector3d accel_white_noise(2.3707*2.3707e-4, 2.4575*2.4575e-4, 2.5139*2.5139e-4); const Vector3d accel_white_noise ( 2.3707*2.3707e-4, 2.4575*2.4575e-4, 2.5139*2.5139e-4);
const Vector3d gps_pos_noise = Vector3d::Ones() *10 *10 ;
const Vector3d gps_speed_noise = Vector3d::Ones() * 0.1* 0.1;
/*
*
* HEADERS
*
*/
/* STM32 Communication */
static void main_periodic(int my_sig_num);
static void main_trick_libevent(void); static void main_trick_libevent(void);
static void on_foo_event(int fd, short event __attribute__((unused)), void *arg); static void on_foo_event(int fd, short event __attribute__((unused)), void *arg);
static struct event foo_event; static struct event foo_event;
static void main_rawlog_init(const char* filename);
static void main_rawlog_dump(void);
static void main_init(void);
static void init_ins_state(void);
static void set_reference_direction(void);
static void main_periodic(int my_sig_num);
static uint8_t main_dialog_with_io_proc(void); static uint8_t main_dialog_with_io_proc(void);
static void main_run_ins(uint8_t);
/* libeknav */
#if RUN_FILTER
static void main_run_ins(uint8_t);
#endif
/* Logging */ /* Logging */
#define IMU_LOG_FILE "/tmp/log_test3.bin" #define IMU_LOG_FILE "/tmp/log_test3.bin"
#define INS_LOG_FILE "/tmp/log_ins_test3.data" static void main_rawlog_init(const char* filename);
static void main_rawlog_dump(uint8_t);
#if RUN_FILTER
static void print_estimator_state(double); static void print_estimator_state(double);
#define INS_LOG_FILE "/tmp/log_ins_test3.data"
#endif
/* time measurement */ /* time measurement */
#define TIMER CLOCK_MONOTONIC #define TIMER CLOCK_MONOTONIC
double absTime(struct timespec); double absTime(struct timespec);
struct timespec time_diff(struct timespec, struct timespec); struct timespec time_diff(struct timespec, struct timespec);
/* Other */ /* Other */
#define UGLY_ANGLE_IN_RADIANS(degree, arcmin, arcsec) ((degree+arcmin/60+arcsec/3600)*M_PI/180) /** tiny little functions **/
#define COORDS_AS_VECTOR(coords) Vector3d(coords.x, coords.y, coords.z) #define DEFINE_AutopilotMessageCRCFrame_IN_and_OUT(name) \
#define RATES_AS_VECTOR(rates) Vector3d(rates.p,rates.q,rates.r) struct AutopilotMessageCRCFrame name##_in; \
#define ABS(a) ((a<0)?-a:a) struct AutopilotMessageCRCFrame name##_out
#define VECTOR_AS_COORDS(coords, vector) { coords.x = vector(0); coords.y = vector(1); coords.z = vector(2);} void printmag(void);
void printgps(void);
/** Sensors **/
#define COPY_RATES_ACCEL_TO_IMU_FLOAT(pointer){ \
RATES_FLOAT_OF_BFP(imu_float.gyro, pointer->gyro); \
ACCELS_FLOAT_OF_BFP(imu_float.accel, pointer->accel); \
}
#define COPY_MAG_TO_IMU_FLOAT(pointer) MAGS_FLOAT_OF_BFP(imu_float.mag, pointer->mag)
#define COPY_GPS_TO_IMU(pointer){ \
VECT3_COPY(imu_ecef_pos, pointer->ecef_pos); \
VECT3_COPY(imu_ecef_vel, pointer->ecef_vel); \
}
#define IMU_READY(data_valid) (data_valid & (1<<VI_IMU_DATA_VALID))
#define GPS_READY(data_valid) (data_valid & (1<<VI_GPS_DATA_VALID))
#define MAG_READY(data_valid) (data_valid & (1<<VI_MAG_DATA_VALID))
#define CLOSE_TO_GRAVITY(accel) (ABS(FLOAT_VECT3_NORM(accel)-GRAVITY)<MAX_DISTANCE_FROM_GRAVITY_FOR_UPDATE)
/** Converions **/
/* copied and modified form pprz_geodetic */
#define NED_TO_ECEF_MAT(lla, mat) { \
const double sin_lat = sin(lla.lat); \
const double cos_lat = cos(lla.lat); \
const double sin_lon = sin(lla.lon); \
const double cos_lon = cos(lla.lon); \
MAT33_ROW(mat, 0, -sin_lat*cos_lon, -sin_lon, -cos_lat*cos_lon); \
MAT33_ROW(mat, 1, sin_lat*sin_lon, cos_lon, -cos_lat*sin_lon); \
MAT33_ROW(mat, 2, cos_lat , 0 , -sin_lat ); \
}