Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-02 05:17:03 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

changed the output

Browse Source
This commit is contained in:
Martin Dieblich
2010-10-28 12:21:53 +00:00
parent ed8397780d
commit 7be3ad1151
3 changed files with 34 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/fms/libeknav/libeknav_from_log.cpp
+28 -23
View File
@@ -46,13 +46,13 @@ static void main_init(void) {
printf("FILTER output will be in "); printf("FILTER output will be in ");
#if FILTER_OUTPUT_IN_NED #if FILTER_OUTPUT_IN_NED
printf("NED\n"); printf("NED\n");
#else #else /* FILTER_OUTPUT_IN_ECEF */
printf("ECEF\n"); printf("ECEF\n");
#endif #endif /* FILTER_OUTPUT_IN_NED / ECEF */
#if UPDATE_WITH_GRAVITY #if UPDATE_WITH_GRAVITY
printf("the orientation becomes UPDATED with the GRAVITY\n"); printf("the orientation becomes UPDATED with the GRAVITY\n");
#endif #endif /* UPDATE_WITH_GRAVITY */
init_ins_state(); init_ins_state();
set_reference_direction(); set_reference_direction();
@@ -91,20 +91,20 @@ static struct raw_log_entry first_entry_after_initialisation(int file_descriptor
uint8_t read_ok; uint8_t read_ok;
#if WITH_GPS #if WITH_GPS
struct raw_log_entry e = next_GPS(file_descriptor); struct raw_log_entry e = next_GPS(file_descriptor);
#else #else /* WITH_GPS */
struct raw_log_entry e = read_raw_log_entry(file_descriptor, &read_ok); struct raw_log_entry e = read_raw_log_entry(file_descriptor, &read_ok);
pos_0_ecef = Vector3d(4627578.56, 119659.25, 4373248.00); pos_0_ecef = Vector3d(4627578.56, 119659.25, 4373248.00);
pos_cov_0 = Vector3d::Ones()*100; pos_cov_0 = Vector3d::Ones()*100;
speed_0_ecef = Vector3d::Zero(); speed_0_ecef = Vector3d::Zero();
speed_cov_0 = Vector3d::Ones(); speed_cov_0 = Vector3d::Ones();
#endif // WITH_GPS #endif /* WITH_GPS */
#ifdef EKNAV_FROM_LOG_DEBUG #ifdef EKNAV_FROM_LOG_DEBUG
int imu_ready = 0, int imu_ready = 0,
mag_ready = 0, mag_ready = 0,
baro_ready = 0, baro_ready = 0,
gps_ready = 0; gps_ready = 0;
#endif #endif /* EKNAV_FROM_LOG_DEBUG */
for(read_ok = 1; (read_ok) && NOT_ENOUGH_MEASUREMENTS(imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements); e = read_raw_log_entry(file_descriptor, &read_ok)){ for(read_ok = 1; (read_ok) && NOT_ENOUGH_MEASUREMENTS(imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements); e = read_raw_log_entry(file_descriptor, &read_ok)){
if(IMU_READY(e.message.valid_sensors)){ if(IMU_READY(e.message.valid_sensors)){
@@ -165,7 +165,7 @@ static struct raw_log_entry first_entry_after_initialisation(int file_descriptor
gps_ready = 1; gps_ready = 1;
} }
} }
#endif #endif /* EKNAV_FROM_LOG_DEBUG */
} }
// setting the covariance // setting the covariance
gravity.weight_of_the_measurement *= imu_measurements; gravity.weight_of_the_measurement *= imu_measurements;
@@ -177,7 +177,7 @@ static struct raw_log_entry first_entry_after_initialisation(int file_descriptor
#ifdef EKNAV_FROM_LOG_DEBUG #ifdef EKNAV_FROM_LOG_DEBUG
DISPLAY_FLOAT_RMAT(" B", attitude_profile_matrix); DISPLAY_FLOAT_RMAT(" B", attitude_profile_matrix);
DISPLAY_FLOAT_RMAT("sigmaB", sigmaB); DISPLAY_FLOAT_RMAT("sigmaB", sigmaB);
#endif #endif /* EKNAV_FROM_LOG_DEBUG */
// setting the initial orientation // setting the initial orientation
q_ned2body = estimated_attitude(attitude_profile_matrix, 1000, 1e-6, sigmaB, &sigma_q); q_ned2body = estimated_attitude(attitude_profile_matrix, 1000, 1e-6, sigmaB, &sigma_q);
@@ -227,19 +227,15 @@ static void main_run_ins(uint8_t data_valid) {
// use the gravity as reference // use the gravity as reference
ins.obs_vector(ins.avg_state.position.normalized(), VECT3_AS_VECTOR3D(imu_float.accel), accelerometer_noise.norm()); ins.obs_vector(ins.avg_state.position.normalized(), VECT3_AS_VECTOR3D(imu_float.accel), accelerometer_noise.norm());
} }
#endif #endif /* UPDATE_WITH_GRAVITY */
if(BARO_READY(data_valid)){ if(BARO_READY(data_valid)){
ins.obs_baro_report(baro_0_height+imu_baro_height, baro_noise); ins.obs_baro_report(baro_0_height+imu_baro_height, baro_noise);
} } // comment out multiple lines */
if(GPS_READY(data_valid)){ if(GPS_READY(data_valid)){
ins.obs_gps_pv_report(VECT3_AS_VECTOR3D(imu_ecef_pos)/100, VECT3_AS_VECTOR3D(imu_ecef_vel)/100, 10*gps_pos_noise, 10*gps_speed_noise); ins.obs_gps_pv_report(VECT3_AS_VECTOR3D(imu_ecef_pos)/100, VECT3_AS_VECTOR3D(imu_ecef_vel)/100, 10*gps_pos_noise, 10*gps_speed_noise);
} // comment out multiple lines */
Matrix<double, 3, 3> ecef2enu;
RMAT_TO_EIGENMATRIX(ecef2enu,current_ltp.ltp_of_ecef.m);
}
} }
@@ -322,8 +318,9 @@ Quaterniond ecef2body_from_pprz_ned2body(Vector3d ecef_pos, struct DoubleQuat q_
ltp_def_from_ecef_d(&current_ltp, &ecef_pos_pprz); ltp_def_from_ecef_d(&current_ltp, &ecef_pos_pprz);
DOUBLE_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef); DOUBLE_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
QUAT_ENU_FROM_TO_NED(q_ecef2enu, q_ecef2ned); QUAT_ENU_FROM_TO_NED(q_ecef2enu, q_ecef2ned);
//FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)
FLOAT_QUAT_COMP_NORM_SHORTEST(q_ecef2body, q_ecef2ned, q_ned2body); // a = ecef b = ned c = body
FLOAT_QUAT_COMP_INV_NORM_SHORTEST(q_ecef2body, q_ecef2ned, q_ned2body);
#ifdef EKNAV_FROM_LOG_DEBUG #ifdef EKNAV_FROM_LOG_DEBUG
printf("Right after initialization:\n"); printf("Right after initialization:\n");
@@ -335,7 +332,7 @@ Quaterniond ecef2body_from_pprz_ned2body(Vector3d ecef_pos, struct DoubleQuat q_
DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ecef2ned); DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ecef2ned);
DISPLAY_DOUBLE_QUAT("\tecef2body quaternion:", q_ecef2body); DISPLAY_DOUBLE_QUAT("\tecef2body quaternion:", q_ecef2body);
DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ecef2body); DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ecef2body);
#endif #endif /* EKNAV_FROM_LOG_DEBUG */
return DOUBLEQUAT_AS_QUATERNIOND(q_ecef2body); return DOUBLEQUAT_AS_QUATERNIOND(q_ecef2body);
} }
@@ -425,22 +422,30 @@ static void print_estimator_state(double time) {
int32_t z = pos_ned.z/0.0039; int32_t z = pos_ned.z/0.0039;
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);
#if 0
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);
FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body); // q_enu2body = q_ecef2body * (q_ecef2enu)^* FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body); // q_enu2body = q_ecef2body * (q_ecef2enu)^*
FLOAT_QUAT_COMP(q_ned2body, q_ned2enu, q_enu2body) // q_ned2body = q_enu2body * q_ned2enu FLOAT_QUAT_COMP(q_ned2body, q_ned2enu, q_enu2body); // q_ned2body = q_enu2body * q_ned2enu
#else /* if 0 */
QUATERNIOND_AS_DOUBLEQUAT(q_ecef2body, ins.avg_state.orientation);
DOUBLE_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body);
QUAT_ENU_FROM_TO_NED(q_enu2body, q_ned2body);
#endif /* if 0 */
struct FloatEulers e; struct FloatEulers e;
FLOAT_EULERS_OF_QUAT(e, q_ned2body); FLOAT_EULERS_OF_QUAT(e, q_ned2body);
#if PRINT_EULER_NED #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); 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 #endif /* PRINT_EULER_NED */
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)),
@@ -449,7 +454,7 @@ static void print_estimator_state(double time) {
sqrt(ins.cov( 9, 9)), sqrt(ins.cov(10,10)), sqrt(ins.cov(11,11))); sqrt(ins.cov( 9, 9)), sqrt(ins.cov(10,10)), sqrt(ins.cov(11,11)));
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));
#else #else /* FILTER_OUTPUT_IN_ECEF */
int32_t xdd = 0; int32_t xdd = 0;
int32_t ydd = 0; int32_t ydd = 0;
int32_t zdd = 0; int32_t zdd = 0;
@@ -476,5 +481,5 @@ static void print_estimator_state(double time) {
sqrt(ins.cov( 6, 6)), sqrt(ins.cov( 7, 7)), sqrt(ins.cov( 8, 8)), sqrt(ins.cov( 6, 6)), sqrt(ins.cov( 7, 7)), sqrt(ins.cov( 8, 8)),
sqrt(ins.cov( 9, 9)), sqrt(ins.cov(10,10)), sqrt(ins.cov(11,11))); sqrt(ins.cov( 9, 9)), sqrt(ins.cov(10,10)), sqrt(ins.cov(11,11)));
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 /* FILTER_OUTPUT_IN_NED / ECEF */
} }
sw/airborne/fms/libeknav/libeknav_from_log.hpp
+2 -3
View File
@@ -51,8 +51,6 @@
#define PRINT_GPS 0 #define PRINT_GPS 0
#define PRINT_EULER_NED 0 #define PRINT_EULER_NED 0
/** baro-sensor **/
/** geodetic **/ /** geodetic **/
#define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) VECT3_ASSIGN(ref, 0.51562740288882, -0.05707735220832, 0.85490967783446) #define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) VECT3_ASSIGN(ref, 0.51562740288882, -0.05707735220832, 0.85490967783446)
@@ -90,7 +88,8 @@ Vector3d speed_cov_0 = Vector3d::Ones()*0.1;
// NOTE: Measured during hovering in the air. Movement in the range of a 1 m³ cube with (approx.) max. 0.2 m/s speed. // NOTE: Measured during hovering in the air. Movement in the range of a 1 m³ cube with (approx.) max. 0.2 m/s speed.
/// IMU /// IMU
const Vector3d gyroscope_noise ( 1.0449e-1, 1.1191e-1, 4.5906e-2 ); const Vector3d gyroscope_noise ( 1.0449e-1, 1.1191e-1, 4.5906e-2 );
const Vector3d gyro_stability_noise ( 1.0000e-6, 1.0000e-6, 1.0000e-6 ); //const Vector3d gyroscope_noise ( 1.0000e-1, 1.0000e-1, 1.0000e-1 );
const Vector3d gyro_stability_noise ( 1.0000e-3, 1.0000e-3, 1.0000e-3 );
const Vector3d accelerometer_noise ( 2.5457e+0, 1.8242e+0, 1.5660e+0 ); const Vector3d accelerometer_noise ( 2.5457e+0, 1.8242e+0, 1.5660e+0 );
const unsigned short imu_frequency = 512; const unsigned short imu_frequency = 512;
sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
+1 -1
View File
@@ -157,7 +157,7 @@
*/ */
#define VECTOR_AS_VECT3(coords, vector) { coords.x = vector(0); coords.y = vector(1); coords.z = vector(2);} #define VECTOR_AS_VECT3(coords, vector) { coords.x = vector(0); coords.y = vector(1); coords.z = vector(2);}
#define QUATERNIOND_AS_DOUBLEQUAT(doublequat, quaterniond) {doublequat.qi = quaterniond.w(); doublequat.qx = quaterniond.x(); doublequat.qy = quaterniond.y(); doublequat.qz = quaterniond.z();} #define QUATERNIOND_AS_DOUBLEQUAT(doublequat, quaterniond) {(doublequat).qi = (quaterniond).w(); (doublequat).qx = (quaterniond).x(); (doublequat).qy = (quaterniond).y(); (doublequat).qz = (quaterniond).z();}
#define PPRZ_LLA_TO_EIGEN_ECEF(lla, ecef){ \ #define PPRZ_LLA_TO_EIGEN_ECEF(lla, ecef){ \
struct EcefCoor_f ecef_pprz; \ struct EcefCoor_f ecef_pprz; \
ecef_of_lla_f(&ecef_pprz, &lla); \ ecef_of_lla_f(&ecef_pprz, &lla); \