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 29 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>
HOST=auto1
USER=root
PAPARAZZI_INC = -I$(PAPARAZZI_HOME)/var/$(AIRCRAFT) \
-I$(PAPARAZZI_SRC)/sw/airborne \
-I$(PAPARAZZI_SRC)/sw/include
doc/pprz_geodetic/ned_enu.tex
+2 -2
View File
@@ -30,7 +30,7 @@ p_{ENU} \qquad p_{ENU} = \begin{pmatrix}
or directly switching the values
\begin{equation}
\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}
\inHfile{ENU\_OF\_TO\_NED(po, pi)}{pprz\_geodetic}
\inHfile{INT32\_VECT2\_ENU\_OF\_NED(o, i)}{pprz\_geodetic\_int}
@@ -50,4 +50,4 @@ or directly switching the values
\input{transformations/ecef2ned_enu}
\subsubsection*{from LLA}
\input{transformations/lla2ned_enu}
\input{transformations/lla2ned_enu}
sw/airborne/fms/libeknav/Makefile
+1 -1
View File
@@ -4,9 +4,9 @@ raw_log_to_ascii: raw_log_to_ascii.c
fetch_log:
scp @auto1:/tmp/log_ins_test3.data .
scp @auto1:/tmp/log_test3.bin .
./raw_log_to_ascii > bla.dat
scp @auto1:/tmp/log_ins_test3.data .
clean:
-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(),
const Vector3d& vel_estimate = Vector3d::Zero());
/* //Old one without orientation_init()
basic_ins_qkf(const Vector3d& pos_estimate,
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 "assertions.hpp"
#ifdef TIME_OPS
#include "timer.hpp"
# include <iostream>
//#include "timer.hpp"
#include <iostream>
#endif
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
// of the vehicle orientation and the translational reference frame.
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_gravity = _this.avg_state.position.normalized()*(-9.81);
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
// This form works well with zero static acceleration.
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"
struct raw_log_entry {
struct __attribute__ ((packed)) raw_log_entry{
float time;
struct FloatRates gyro;
struct FloatVect3 accel;
struct FloatVect3 mag;
struct FloatVect3 ecef_pos;
struct FloatVect3 ecef_vel;
uint8_t data_valid;
};
#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){
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->accel.x, entry->accel.y, entry->accel.z);
printf("%+f %+f %+f\t", entry->mag.x, entry->mag.y, entry->mag.z);
sw/airborne/fms/libeknav/test_libeknav_4.cpp
+107 -95
View File
@@ -3,17 +3,18 @@
#include <stdlib.h>
struct timespec start, prev;
FILE* ins_logfile; // note: initilaized in init_ins_state
unsigned int counter;
#if RUN_FILTER
//useless initialization (I hate C++)
static basic_ins_qkf ins = basic_ins_qkf(Vector3d::Zero(), 0, 0, 0,
Vector3d::Zero(), Vector3d::Zero(), Vector3d::Zero());
// import most common Eigen types
#endif
USING_PART_OF_NAMESPACE_EIGEN
int main(int, char *[]) {
@@ -40,12 +41,20 @@ int main(int, char *[]) {
static void main_init(void) {
printf("Filter output will be in ");
#if FILTER_OUTPUT_IN_NED
printf("NED\n");
#if RUN_FILTER
printf("FILTER output will be in ");
#if FILTER_OUTPUT_IN_NED
printf("NED\n");
#else
printf("ECEF\n");
#endif
#else
printf("ECEF\n");
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");
@@ -63,9 +72,10 @@ static void main_init(void) {
TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
return;
}
#if RUN_FILTER
init_ins_state();
set_reference_direction();
#endif
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();
#if RUN_FILTER
main_run_ins(data_valid);
main_rawlog_dump();
#endif
main_rawlog_dump(data_valid);
}
static uint8_t main_dialog_with_io_proc() {
struct AutopilotMessageCRCFrame msg_in;
struct AutopilotMessageCRCFrame msg_out;
uint8_t crc_valid;
DEFINE_AutopilotMessageCRCFrame_IN_and_OUT(message);
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];
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)){
RATES_FLOAT_OF_BFP(imu_float.gyro, in->gyro);
ACCELS_FLOAT_OF_BFP(imu_float.accel, in->accel);
if(IMU_READY(in->valid_sensors)){
COPY_RATES_ACCEL_TO_IMU_FLOAT(in);
}
if(in->valid_sensors & (1<< VI_MAG_DATA_VALID)){
MAGS_FLOAT_OF_BFP(imu_float.mag, in->mag);
printf("MAG: %f %f %f\n", imu_float.mag.x, imu_float.mag.y, imu_float.mag.z);
if(MAG_READY(in->valid_sensors)){
#if SYNTHETIC_MAG_MODE
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)){
VECT3_COPY(imu_ecef_pos, in->ecef_pos);
printf("GPS: %d %d %d\n", imu_ecef_pos.x, imu_ecef_pos.y, imu_ecef_pos.z);
VECT3_COPY(imu_ecef_vel, in->ecef_vel);
if(GPS_READY(in->valid_sensors)){
COPY_GPS_TO_IMU(in);
#if PRINT_GPS
printgps();
#endif
}
return in->valid_sensors;
}
#if RUN_FILTER
static void main_run_ins(uint8_t data_valid) {
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?
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)){
//ins.obs_vector(reference_direction, COORDS_AS_VECTOR(imu_float.mag), mag_noise);
if(MAG_READY(data_valid)){
ins.obs_vector(reference_direction, VECT3_AS_VECTOR3D(imu_float.mag), mag_noise);
}
#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
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
if(data_valid & (1<<VI_GPS_DATA_VALID)){
const Vector3d gps_pos_noise = Vector3d::Ones() *10*10;
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);
if(GPS_READY(data_valid)){
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);
}
print_estimator_state(absTime(time_diff(now, start)));
}
#endif
@@ -172,72 +188,67 @@ static void on_foo_event(int fd __attribute__((unused)), short event __attribute
}
#if RUN_FILTER
static void init_ins_state(void){
ins_logfile = fopen(INS_LOG_FILE, "w");
LLA_ASSIGN(pos_0_lla, TOULOUSE_LATTITUDE, TOULOUSE_LONGITUDE, TOULOUSE_HEIGHT)
struct EcefCoor_f pos_0_ecef_pprz;
ecef_of_lla_f(&pos_0_ecef_pprz, &pos_0_lla);
pos_0_ecef = COORDS_AS_VECTOR(pos_0_ecef_pprz);
PPRZ_LLA_TO_EIGEN_ECEF(pos_0_lla, pos_0_ecef);
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.gyro_bias = Vector3d::Zero();
ins.avg_state.position = pos_0_ecef;
ins.avg_state.gyro_bias = Vector3d::Zero();
ins.avg_state.orientation = Quaterniond::Identity();
ins.avg_state.velocity = speed_0_ecef;
ins.avg_state.velocity = speed_0_ecef;
Matrix<double, 12, 1> diag_cov;
diag_cov << Vector3d::Ones()*bias_cov_0*bias_cov_0,
Vector3d::Ones()*M_PI*M_PI*0.5,
Vector3d::Ones()*pos_cov_0*pos_cov_0,
Vector3d::Ones()*speed_cov_0*speed_cov_0;
diag_cov << Vector3d::Ones() * bias_cov_0 * bias_cov_0 ,
Vector3d::Ones() * M_PI*0.5 * M_PI*0.5 ,
Vector3d::Ones() * pos_cov_0 * pos_cov_0 ,
Vector3d::Ones() * speed_cov_0 * speed_cov_0;
ins.cov = diag_cov.asDiagonal();
}
static void set_reference_direction(void){
struct DoubleVect3 ref_dir_ned,
ref_dir_ecef;
EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned); // the "true" magnetic field
//LAB_REFERENCE(ref_dir_ned); // measured in the LAB
struct NedCoor_d ref_dir_ned;
struct EcefCoor_d pos_0_ecef_pprz,
ref_dir_ecef;
EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned);
struct DoubleRMat ned2ecef;
/* copied and modified form pprz_geodetic */
const double sin_lat = sin(pos_0_lla.lat);
const double cos_lat = cos(pos_0_lla.lat);
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;
struct LtpDef_d current_ltp;
VECTOR_AS_VECT3(pos_0_ecef_pprz, pos_0_ecef);
ltp_def_from_ecef_d(&current_ltp, &pos_0_ecef_pprz);
ecef_of_ned_vect_d(&ref_dir_ecef, &current_ltp, &ref_dir_ned);
RMAT_VECT3_MUL(ref_dir_ecef, ned2ecef, ref_dir_ned);
//MAT33_VECT3_TRANSP_MUL(ref_dir_ecef, ned2ecef, ref_dir_ned);
reference_direction = COORDS_AS_VECTOR(ref_dir_ecef);
// old transformation:
//struct DoubleRMat ned2ecef;
//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);
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);
}
/* helpstuff
*
*
*
*/
/* time measurement */
/** time measurement **/
double absTime(struct timespec T){
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;
}
/* Logging
*
*
*
*/
/** Logging **/
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;
clock_gettime(TIMER, &now);
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.ecef_pos, imu_ecef_pos);
VECT3_COPY(e.ecef_vel, imu_ecef_vel);
e.data_valid = data_valid;
write(raw_log_fd, &e, sizeof(e));
}
#if RUN_FILTER
static void print_estimator_state(double time) {
#if FILTER_OUTPUT_IN_NED
@@ -298,9 +307,9 @@ static void print_estimator_state(double time) {
q_ned2enu,
q_ned2body;
VECTOR_AS_COORDS(pos_ecef,pos_0_ecef);
VECTOR_AS_COORDS(cur_pos_ecef,ins.avg_state.position);
VECTOR_AS_COORDS(cur_vel_ecef,ins.avg_state.velocity);
VECTOR_AS_VECT3(pos_ecef,pos_0_ecef);
VECTOR_AS_VECT3(cur_pos_ecef,ins.avg_state.position);
VECTOR_AS_VECT3(cur_vel_ecef,ins.avg_state.velocity);
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);
QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(),
ins.avg_state.orientation.y(), ins.avg_state.orientation.z());
QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), -ins.avg_state.orientation.x(),
-ins.avg_state.orientation.y(), -ins.avg_state.orientation.z());
QUAT_ASSIGN(q_ned2enu, 0, M_SQRT1_2, M_SQRT1_2, 0);
FLOAT_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
@@ -332,7 +341,10 @@ static void print_estimator_state(double time) {
struct FloatEulers e;
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 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)),
@@ -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);
struct FloatQuat q;
QUAT_ASSIGN(q, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(),
struct FloatQuat q_ecef2body;
QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(),
ins.avg_state.orientation.y(), ins.avg_state.orientation.z());
struct FloatEulers e;
FLOAT_EULERS_OF_QUAT(e, q);
struct FloatEulers e_ecef2body;
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,
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)),
@@ -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));
#endif
}
#endif
sw/airborne/fms/libeknav/test_libeknav_4.hpp
+105 -63
View File
@@ -4,6 +4,7 @@
#include <Eigen/Core>
#include "ins_qkf.hpp"
#include "paparazzi_eigen_conversion.h"
#include <stdint.h>
#include <event.h>
@@ -32,106 +33,147 @@ extern "C" {
#include "math/pprz_geodetic_double.c"
#include <math.h>
#define FILTER_OUTPUT_IN_NED 1
/* constants */
/** Compilation-control **/
#define RUN_FILTER 1
#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
/*
*
* Initialization
*
*/
/* initial state */
/** geodetic **/
//Toulouse Lat: 43° 35' 24'' Lon: 1° 25' 48''
#define TOULOUSE_LATTITUDE UGLY_ANGLE_IN_RADIANS(43,35,24)
#define TOULOUSE_LONGITUDE UGLY_ANGLE_IN_RADIANS(1,25,48)
#define TOULOUSE_LATTITUDE ARCSEC_ACRMIN_ANGLE_IN_RADIANS(43,35,24)
#define TOULOUSE_LONGITUDE ARCSEC_ACRMIN_ANGLE_IN_RADIANS(1,25,48)
#define TOULOUSE_HEIGHT 0
//Toulouse Declination is 22' West and Inclination 59° 8' Down
#define TOULOUSE_DECLINATION -UGLY_ANGLE_IN_RADIANS(0,22,0)
#define TOULOUSE_INCLINATION -UGLY_ANGLE_IN_RADIANS(59,8,0)
#define TOULOUSE_DECLINATION -ARCSEC_ACRMIN_ANGLE_IN_RADIANS(0,22,0)
#define TOULOUSE_INCLINATION -ARCSEC_ACRMIN_ANGLE_IN_RADIANS(59,8,0)
struct LlaCoor_f pos_0_lla;
Vector3d pos_0_ecef;
Vector3d speed_0_ecef;
// Vector3d orientation(0., 0., 0.);
Vector3d bias_0(0., 0., 0.);
/**
* 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
*/
/** magnetic field
** 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
#define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) { \
ref.z = sin(TOULOUSE_INCLINATION); \
double h = sqrt(1-ref.z*ref.z); \
ref.x = h*cos(TOULOUSE_DECLINATION); \
ref.y = h*sin(TOULOUSE_DECLINATION); \
ref.z = sin(TOULOUSE_INCLINATION); \
double h = sqrt(1-ref.z*ref.z); \
ref.x = h*cos(TOULOUSE_DECLINATION); \
ref.y = h*sin(TOULOUSE_DECLINATION); \
}
#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
// mean of the measurment data
#define LAB_REFERENCE(ref) VECT3_ASSIGN(ref, -0.22496030821134, 0.70578892222179, 0.67175505729281)
/** other **/
#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 speed_cov_0 = 3.;
// const double orientation_cov_0 = RadOfDeg(5.)*RadOfDeg(5.);
const double bias_cov_0 = 0.447;
const double mag_noise = std::pow(5 / 180.0 * M_PI, 2);
/* system noise */
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_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);
/*
*
* HEADERS
*
*/
/* system noise */
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_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 gps_pos_noise = Vector3d::Ones() *10 *10 ;
const Vector3d gps_speed_noise = Vector3d::Ones() * 0.1* 0.1;
/* STM32 Communication */
static void main_periodic(int my_sig_num);
static void main_trick_libevent(void);
static void on_foo_event(int fd, short event __attribute__((unused)), void *arg);
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 void main_run_ins(uint8_t);
/* libeknav */
#if RUN_FILTER
static void main_run_ins(uint8_t);
#endif
/* Logging */
#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);
#define INS_LOG_FILE "/tmp/log_ins_test3.data"
#endif
/* time measurement */
#define TIMER CLOCK_MONOTONIC
double absTime(struct timespec);
struct timespec time_diff(struct timespec, struct timespec);
/* Other */
#define UGLY_ANGLE_IN_RADIANS(degree, arcmin, arcsec) ((degree+arcmin/60+arcsec/3600)*M_PI/180)
#define COORDS_AS_VECTOR(coords) Vector3d(coords.x, coords.y, coords.z)
#define RATES_AS_VECTOR(rates) Vector3d(rates.p,rates.q,rates.r)
#define ABS(a) ((a<0)?-a:a)
#define VECTOR_AS_COORDS(coords, vector) { coords.x = vector(0); coords.y = vector(1); coords.z = vector(2);}
/** tiny little functions **/
#define DEFINE_AutopilotMessageCRCFrame_IN_and_OUT(name) \
struct AutopilotMessageCRCFrame name##_in; \
struct AutopilotMessageCRCFrame name##_out
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 ); \
}