[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6148] added a filter running from a file and a filt
|
From: |
Martin Dieblich |
|
Subject: |
[paparazzi-commits] [6148] added a filter running from a file and a filter for conversions between eigen and ppaparazzi . |
|
Date: |
Tue, 12 Oct 2010 16:54:34 +0000 |
Revision: 6148
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6148
Author: mdieblich
Date: 2010-10-12 16:54:33 +0000 (Tue, 12 Oct 2010)
Log Message:
-----------
added a filter running from a file and a filter for conversions between eigen
and ppaparazzi.
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/Poine/test_libeknav.xml
paparazzi3/trunk/sw/airborne/fms/libeknav/Makefile
paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp
paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.hpp
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp
paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
Modified: paparazzi3/trunk/conf/airframes/Poine/test_libeknav.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/test_libeknav.xml 2010-10-12
12:15:12 UTC (rev 6147)
+++ paparazzi3/trunk/conf/airframes/Poine/test_libeknav.xml 2010-10-12
16:54:33 UTC (rev 6148)
@@ -59,6 +59,16 @@
test4.CXXFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageVIUp
-DOVERO_LINK_MSG_DOWN=AutopilotMessageVIDown
test4.srcs += fms/fms_spi_link.c
+ # libeknav_from_log: first, this here
+ filter.ARCHDIR = omap
+ filter.CXXFLAGS += $(LIB_EIGEN_CFLAGS)
+ filter.CXXFLAGS += $(PAPARAZZI_INC)
+ filter.cpp_srcs = fms/libeknav/libeknav_from_log.cpp
+ filter.CXXFLAGS += $(LIBEKNAV_CFLAGS)
+ filter.cpp_srcs += $(LIBEKNAV_SRCS)
+ filter.LDFLAGS += -levent -lm
+ filter.CXXFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageVIUp
-DOVERO_LINK_MSG_DOWN=AutopilotMessageVIDown
+
# test network based telemetry on overo (using udp_transport2/messages2)
overo_test_telemetry2.ARCHDIR = omap
overo_test_telemetry2.CFLAGS += $(PAPARAZZI_INC)
Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/Makefile
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/Makefile 2010-10-12 12:15:12 UTC
(rev 6147)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/Makefile 2010-10-12 16:54:33 UTC
(rev 6148)
@@ -8,5 +8,8 @@
./raw_log_to_ascii > bla.dat
scp @auto1:/tmp/log_ins_test3.data .
+fetch_raw_data:
+ scp @auto1:/home/root/log_ins_test3.data .
+
clean:
-rm -f *.o *~ *.d
Added: paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp
2010-10-12 16:54:33 UTC (rev 6148)
@@ -0,0 +1,235 @@
+
+#include "libeknav_from_log.hpp"
+
+
+FILE* ins_logfile; // note: initilaized in init_ins_state
+
+//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
+USING_PART_OF_NAMESPACE_EIGEN
+
+int main(int, char *argv[]) {
+
+ std::cout << "test libeknav 3" << std::endl;
+ main_init();
+
+
+ int raw_log_fd = open(argv[1], O_RDONLY);
+
+ if (raw_log_fd == -1) {
+ perror("opening log\n");
+ return -1;
+ }
+
+ while (1) {
+ struct raw_log_entry e;
+ ssize_t nb_read = read(raw_log_fd, &e, sizeof(e));
+ if (nb_read != sizeof(e)) break;
+
+ COPY_RATES_ACCEL_TO_IMU_FLOAT(e);
+ COPY_MAG_TO_IMU_FLOAT(e);
+ COPY_GPS_TO_IMU(e);
+ main_run_ins(e.data_valid);
+ print_estimator_state(e.time);
+ }
+
+ return 0;
+
+}
+
+
+static void main_init(void) {
+ printf("FILTER output will be in ");
+ #if FILTER_OUTPUT_IN_NED
+ printf("NED\n");
+ #else
+ printf("ECEF\n");
+ #endif
+
+ #if UPDATE_WITH_GRAVITY
+ printf("the orientation becomes UPDATED with the GRAVITY\n");
+ #endif
+
+ init_ins_state();
+ set_reference_direction();
+
+}
+
+static void main_run_ins(uint8_t data_valid) {
+
+ double dt_imu_freq = 0.001953125; // 1/512; // doesn't work?
+ ins.predict(RATES_AS_VECTOR3D(imu_float.gyro),
VECT3_AS_VECTOR3D(imu_float.accel), dt_imu_freq);
+
+ if(MAG_READY(data_valid)){
+ ins.obs_vector(reference_direction,
VECT3_AS_VECTOR3D(imu_float.mag), mag_noise);
+ }
+
+ #if UPDATE_WITH_GRAVITY
+ if(CLOSE_TO_GRAVITY(imu_float.accel)){
+ // use the gravity as reference
+ ins.obs_vector(ins.avg_state.position.normalized(),
VECT3_AS_VECTOR3D(imu_float.accel), 1.0392e-3);
+ }
+ #endif
+
+ 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);
+ }
+
+}
+
+
+
+
+static void init_ins_state(void){
+
+ ins_logfile = fopen(INS_LOG_FILE, "w");
+
+ LLA_ASSIGN(pos_0_lla, TOULOUSE_LATTITUDE, TOULOUSE_LONGITUDE,
TOULOUSE_HEIGHT)
+ 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));
+
+ 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;
+
+
+ Matrix<double, 12, 1> diag_cov;
+ 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 NedCoor_d ref_dir_ned;
+ struct EcefCoor_d pos_0_ecef_pprz,
+
ref_dir_ecef;
+ EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned);
+
+ struct LtpDef_d current_ltp;
+ VECTOR_AS_VECT3(pos_0_ecef_pprz, pos_0_ecef);
+ ltp_def_from_ecef_d(¤t_ltp, &pos_0_ecef_pprz);
+ ecef_of_ned_vect_d(&ref_dir_ecef, ¤t_ltp, &ref_dir_ned);
+
+ // THIS SOMEWHERE ELSE!
+ DoubleQuat initial_orientation;
+ FLOAT_QUAT_ZERO(initial_orientation);
+ ENU_NED_transformation(current_ltp.ltp_of_ecef);
+ DOUBLE_QUAT_OF_RMAT(initial_orientation, current_ltp.ltp_of_ecef);
+ ins.avg_state.orientation =
DOUBLEQUAT_AS_QUATERNIOND(initial_orientation);
+ // THIS SOMEWHERE ELSE! (END)
+
+ // 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).normalized();
+ //reference_direction = Vector3d(1, 0, 0);
+ std::cout <<"reference direction NED : " <<
VECT3_AS_VECTOR3D(ref_dir_ned).transpose() << std::endl;
+ std::cout <<"reference direction ECEF: " <<
reference_direction.transpose() << std::endl;
+}
+
+
+/* helpstuff */
+/** Logging **/
+
+static void print_estimator_state(double time) {
+
+#if FILTER_OUTPUT_IN_NED
+
+ struct LtpDef_d current_ltp;
+ struct EcefCoor_d pos_ecef,
+
cur_pos_ecef,
+
cur_vel_ecef;
+ struct NedCoor_d pos_ned,
+
vel_ned;
+
+ struct DoubleQuat q_ecef2body,
+
q_ecef2enu,
+
q_enu2body,
+
q_ned2enu,
+
q_ned2body;
+
+ 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(¤t_ltp, &pos_ecef);
+
+ ned_of_ecef_point_d(&pos_ned, ¤t_ltp, &cur_pos_ecef);
+ ned_of_ecef_vect_d(&vel_ned, ¤t_ltp, &cur_vel_ecef);
+
+ int32_t xdd = 0;
+ int32_t ydd = 0;
+ int32_t zdd = 0;
+
+ int32_t xd = vel_ned.x/0.0000019073;
+ int32_t yd = vel_ned.y/0.0000019073;
+ int32_t zd = vel_ned.z/0.0000019073;
+
+ int32_t x = pos_ned.x/0.0039;
+ int32_t y = pos_ned.y/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);
+
+ 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);
+ 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
+
+
+ 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)),
+ sqrt(ins.cov( 3, 3)), sqrt(ins.cov( 4, 4)),
sqrt(ins.cov( 5, 5)),
+ 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)));
+ 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
+ int32_t xdd = 0;
+ int32_t ydd = 0;
+ int32_t zdd = 0;
+
+ int32_t xd = ins.avg_state.velocity(0)/0.0000019073;
+ int32_t yd = ins.avg_state.velocity(1)/0.0000019073;
+ int32_t zd = ins.avg_state.velocity(2)/0.0000019073;
+ int32_t x = ins.avg_state.position(0)/0.0039;
+ int32_t y = ins.avg_state.position(1)/0.0039;
+ int32_t z = ins.avg_state.position(2)/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);
+
+ 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_ecef2body;
+ FLOAT_EULERS_OF_QUAT(e_ecef2body, q_ecef2body);
+
+ 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)),
+ 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)));
+ 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
+}
Added: paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
2010-10-12 16:54:33 UTC (rev 6148)
@@ -0,0 +1,151 @@
+//#include <iostream>
+//#include <iomanip>
+
+#include <Eigen/Core>
+
+#include "ins_qkf.hpp"
+#include "paparazzi_eigen_conversion.h"
+#include <stdint.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+#include <event.h>
+
+
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_algebra_double.h"
+#include "math/pprz_geodetic.h"
+#include "math/pprz_geodetic_int.c"
+#include "math/pprz_geodetic_float.c"
+#include "math/pprz_geodetic_double.c"
+#include <math.h>
+
+#include <unistd.h>
+#include <time.h>
+#include "std.h"
+#include "firmwares/rotorcraft/imu.h"
+#include "fms/fms_autopilot_msg.h"
+#include "fms/libeknav/raw_log.h"
+ /* our sensors */
+ struct ImuFloat imu_float;
+ struct EcefCoor_i imu_ecef_pos,
+
imu_ecef_vel;
+
+
+
+
+/* constants */
+/** Compilation-control **/
+#define UPDATE_WITH_GRAVITY 1
+#define SYNTHETIC_MAG_MODE 0
+#define FILTER_OUTPUT_IN_NED 1
+
+#define PRINT_MAG 0
+#define PRINT_GPS 1
+#define PRINT_EULER_NED 0
+
+/** geodetic **/
+//Toulouse Lat: 43° 35' 24'' Lon: 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 -ARCSEC_ACRMIN_ANGLE_IN_RADIANS(0,22,0)
+#define TOULOUSE_INCLINATION -ARCSEC_ACRMIN_ANGLE_IN_RADIANS(59,8,0)
+
+/** 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);
\
+}
+#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.51562740288882, -0.05707735220832, 0.85490967783446)
+#endif
+Vector3d reference_direction;
+
+static void set_reference_direction(void);
+
+/** other **/
+#define GRAVITY 9.81
+#define MAX_DISTANCE_FROM_GRAVITY_FOR_UPDATE 0.03
+
+/* Initialisation */
+static void main_init(void);
+
+/** initial state **/
+struct LlaCoor_f pos_0_lla;
+Vector3d pos_0_ecef;
+Vector3d speed_0_ecef = Vector3d::Zero();
+Vector3d bias_0(0., 0., 0.);
+static void init_ins_state(void);
+
+/** 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);
+const Vector3d gps_pos_noise = Vector3d::Ones() *10 *10 ;
+const Vector3d gps_speed_noise = Vector3d::Ones() * 0.1* 0.1;
+
+
+/* libeknav */
+static void main_run_ins(uint8_t);
+
+
+/* Logging */
+static void print_estimator_state(double);
+#define INS_LOG_FILE "log_ins_test3.data"
+
+
+
+/* Other */
+
+ /** 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)
+
+ /** Conversions **/
+ /* 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 );
\
+}
Added: paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
2010-10-12 16:54:33 UTC (rev 6148)
@@ -0,0 +1,141 @@
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+ * MA 02110-1301, USA.
+ *
+ *
===================================================================
+ * #
#
+ * # Important note:
#
+ * # Please pay respect to the different perspectives that
Eigen #
+ * # uses in comparison to Paparazzi.
#
+ * # See the documentation in doc/pprz_algebra for more
explanation. #
+ * #
#
+ * # As a result...
#
+ * # ... the euler angles are negated
#
+ * # ... rotational matrices are transposed/inverted
#
+ * # ... quaternions are composed.
#
+ * #
#
+ * # Thank you for you attention :-)
#
+ * #
#
+ *
===================================================================
+ */
+
+/*
+ * additional mathematical transformations
+ * (could also be in math/algebra.h)
+ */
+
+#define ARCSEC_ACRMIN_ANGLE_IN_RADIANS(degree, arcmin, arcsec)
((degree+arcmin/60+arcsec/3600)*M_PI/180)
+#define ABS(a) ((a<0)?-a:a)
+#define MAT33_TRANSP(_to,_from) { \
+ MAT33_ELMT((_to),0,0) = MAT33_ELMT((_from),0,0); \
+ MAT33_ELMT((_to),0,1) = MAT33_ELMT((_from),1,0); \
+ MAT33_ELMT((_to),0,2) = MAT33_ELMT((_from),2,0); \
+ MAT33_ELMT((_to),1,0) = MAT33_ELMT((_from),0,1); \
+ MAT33_ELMT((_to),1,1) = MAT33_ELMT((_from),1,1); \
+ MAT33_ELMT((_to),1,2) = MAT33_ELMT((_from),2,1); \
+ MAT33_ELMT((_to),2,0) = MAT33_ELMT((_from),0,2); \
+ MAT33_ELMT((_to),2,1) = MAT33_ELMT((_from),1,2); \
+ MAT33_ELMT((_to),2,2) = MAT33_ELMT((_from),2,2); \
+}
+#define SWAP(a, b){ \
+ typeof (a) temp = a; \
+ a = b;
\
+ b = temp;
\
+}
+
+#define MAT33_ROW(mat, row, value0, value1, value2){ \
+ mat[row*3+0] = value0;
\
+ mat[row*3+1] = value1;
\
+ mat[row*3+2] = value2;
\
+}
+
+#define ENU_NED_transformation(mat){ \
+ SWAP(mat.m[0], mat.m[3]); \
+ SWAP(mat.m[1], mat.m[4]); \
+ SWAP(mat.m[2], mat.m[5]); \
+ mat.m[6] = -mat.m[6]; \
+ mat.m[7] = -mat.m[7]; \
+ mat.m[8] = -mat.m[8]; \
+}
+
+#define DOUBLE_QUAT_OF_RMAT(_q, _r) { \
+ const double tr = RMAT_TRACE(_r); \
+ if (tr > 0) { \
+ const double two_qi = sqrtf(1.+tr); \
+ const double four_qi = 2. * two_qi; \
+ _q.qi = 0.5 * two_qi; \
+ _q.qx = (RMAT_ELMT(_r, 1, 2)-RMAT_ELMT(_r, 2, 1))/four_qi; \
+ _q.qy = (RMAT_ELMT(_r, 2, 0)-RMAT_ELMT(_r, 0, 2))/four_qi; \
+ _q.qz = (RMAT_ELMT(_r, 0, 1)-RMAT_ELMT(_r, 1, 0))/four_qi; \
+ /*printf("tr > 0\n");*/ \
+ } \
+ else { \
+ if (RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 1, 1) && \
+ RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 2, 2)) { \
+ const double two_qx = sqrtf(RMAT_ELMT(_r, 0, 0) -RMAT_ELMT(_r, 1, 1) \
+ -RMAT_ELMT(_r, 2, 2) + 1); \
+ const double four_qx = 2. * two_qx; \
+ _q.qi = (RMAT_ELMT(_r, 1, 2)-RMAT_ELMT(_r, 2, 1))/four_qx; \
+ _q.qx = 0.5 * two_qx; \
+ _q.qy = (RMAT_ELMT(_r, 0, 1)+RMAT_ELMT(_r, 1, 0))/four_qx; \
+ _q.qz = (RMAT_ELMT(_r, 2, 0)+RMAT_ELMT(_r, 0, 2))/four_qx; \
+ /*printf("m00 largest\n");*/ \
+ }
\
+ else if (RMAT_ELMT(_r, 1, 1) > RMAT_ELMT(_r, 2, 2)) { \
+ const double two_qy = \
+ sqrtf(RMAT_ELMT(_r, 1, 1) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 2, 2)
+ 1); \
+ const double four_qy = 2. * two_qy; \
+ _q.qi = (RMAT_ELMT(_r, 2, 0) - RMAT_ELMT(_r, 0, 2))/four_qy; \
+ _q.qx = (RMAT_ELMT(_r, 0, 1) + RMAT_ELMT(_r, 1, 0))/four_qy; \
+ _q.qy = 0.5 * two_qy; \
+ _q.qz = (RMAT_ELMT(_r, 1, 2) + RMAT_ELMT(_r, 2, 1))/four_qy; \
+ /*printf("m11 largest\n");*/ \
+ }
\
+ else { \
+ const double two_qz = \
+ sqrtf(RMAT_ELMT(_r, 2, 2) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 1, 1)
+ 1); \
+ const double four_qz = 2. * two_qz; \
+ _q.qi = (RMAT_ELMT(_r, 0, 1)- RMAT_ELMT(_r, 1, 0))/four_qz; \
+ _q.qx = (RMAT_ELMT(_r, 2, 0)+ RMAT_ELMT(_r, 0, 2))/four_qz; \
+ _q.qy = (RMAT_ELMT(_r, 1, 2)+ RMAT_ELMT(_r, 2, 1))/four_qz; \
+ _q.qz = 0.5 * two_qz; \
+ /*printf("m22 largest\n");*/ \
+ }
\
+ } \
+ }
+
+
+/*
+ * transformations from paparazzi to Eigen
+ */
+
+#define VECT3_AS_VECTOR3D(coords) Vector3d(coords.x, coords.y, coords.z)
+#define EULER_AS_VECTOR3D(euler) -Vector3d(euler.phi, euler.theta, euler.psi);
+#define RATES_AS_VECTOR3D(rates) Vector3d(rates.p,rates.q,rates.r)
+#define DOUBLEQUAT_AS_QUATERNIOND(quat) Quaterniond(quat.qi, -quat.qx,
-quat.qy, -quat.qz)
+
+#define RMAT_TO_EIGENMATRIX(Eigen,c) Eigen <<
c[0],c[3],c[6],c[1],c[4],c[7],c[2],c[5],c[8]
+
+
+/*
+ * tranformations from Eigen to paparazzi
+ */
+
+#define VECTOR_AS_VECT3(coords, vector) { coords.x = vector(0); coords.y =
vector(1); coords.z = vector(2);}
+#define PPRZ_LLA_TO_EIGEN_ECEF(lla, ecef){ \
+ struct EcefCoor_f ecef_pprz;
\
+ ecef_of_lla_f(&ecef_pprz, &lla);
\
+ ecef = VECT3_AS_VECTOR3D(ecef_pprz); \
+}
Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp
2010-10-12 12:15:12 UTC (rev 6147)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp
2010-10-12 16:54:33 UTC (rev 6148)
@@ -85,7 +85,10 @@
counter++;
if(counter%128 == 0){
- printf("%f s\n", (double)counter/512);
+ printf("%6.2f s\t", (double)counter/512);
+ if(counter%512 == 0){
+ printf("\n");
+ }
}
uint8_t data_valid = main_dialog_with_io_proc();
Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.hpp
2010-10-12 12:15:12 UTC (rev 6147)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.hpp
2010-10-12 16:54:33 UTC (rev 6148)
@@ -36,13 +36,13 @@
/* constants */
/** Compilation-control **/
-#define RUN_FILTER 1
+#define RUN_FILTER 0
#define UPDATE_WITH_GRAVITY 1
#define SYNTHETIC_MAG_MODE 0
#define FILTER_OUTPUT_IN_NED 1
#define PRINT_MAG 0
-#define PRINT_GPS 1
+#define PRINT_GPS 0
#define PRINT_EULER_NED 0
/** geodetic **/
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6148] added a filter running from a file and a filter for conversions between eigen and ppaparazzi .,
Martin Dieblich <=