diff --git a/conf/Makefile.omap b/conf/Makefile.omap index a45a20b474..51ca86d5eb 100644 --- a/conf/Makefile.omap +++ b/conf/Makefile.omap @@ -62,8 +62,9 @@ CFLAGS += $($(TARGET).CFLAGS) CXX = /opt/paparazzi/omap/overo-oe/tmp/sysroots/i686-linux/usr/armv7a/bin/arm-angstrom-linux-gnueabi-g++ -CXXFLAGS = -pipe -O3 -fshow-column -ffast-math -DEIGEN_DONT_VECTORIZE -fPIC -CXXFLAGS += -DEIGEN_DONT_ALIGN -DNDEBUG -g -ffunction-sections -fdata-sections -DTIME_OPS +CXXFLAGS = -pipe -O3 -fshow-column -ffast-math -fPIC +CXXFLAGS += -g -ffunction-sections -fdata-sections +CXXFLAGS += -mfloat-abi=softfp -mtune=cortex-a8 -mfpu=vfp -march=armv7-a CXXFLAGS += -Wall -Wextra CXXFLAGS += $($(TARGET).CXXFLAGS) @@ -90,7 +91,7 @@ load upload program: $(OBJDIR)/$(TARGET).elf %.elf: $(OBJ_C_OMAP) $(OBJ_CPP_OMAP) @echo LD $@ $(Q)if (expr "$($(TARGET).cpp_srcs)"); \ - then $(CXX) $(CXXFLAGS) $(OBJ_CPP_OMAP) --output $@ $(LDFLAGS) $($(TARGET).LDFLAGS) ; \ + then $(CXX) $(CXXFLAGS) $(OBJ_CPP_OMAP) $(OBJ_C_OMAP) --output $@ $(LDFLAGS) $($(TARGET).LDFLAGS); \ else $(CC) $(CFLAGS) $(OBJ_C_OMAP) --output $@ $(LDFLAGS) $($(TARGET).LDFLAGS); fi # Compile: create object files from C source files diff --git a/conf/airframes/Poine/test_libeknav.xml b/conf/airframes/Poine/test_libeknav.xml index 4e27d22474..bf39cf6151 100644 --- a/conf/airframes/Poine/test_libeknav.xml +++ b/conf/airframes/Poine/test_libeknav.xml @@ -5,11 +5,44 @@ HOST=auto3 - # test 1 + LIB_EIGEN_DIR = /opt/paparazzi/omap/overo-oe/tmp/sysroots/armv7a-angstrom-linux-gnueabi/usr/include/eigen2 + LIB_EIGEN_CFLAGS = -I$(LIB_EIGEN_DIR) -DEIGEN_DONT_VECTORIZE -DEIGEN_DONT_ALIGN -DNDEBUG + + LIBEKNAV_SRCS = fms/libeknav/basic_ins_qkf.cpp \ + fms/libeknav/ins_qkf_predict.cpp \ + fms/libeknav/ins_qkf_observe_vector.cpp \ + fms/libeknav/ins_qkf_observe_gps_pvt.cpp \ + fms/libeknav/ins_qkf_observe_gps_p.cpp +# LIBEKNAV_CFLAGS = -DTIME_OPS + LIBEKNAV_CFLAGS = + + # test 1: how do I build cpp for using libeigen ? test1.ARCHDIR = omap - test1.CXXFLAGS += -I /opt/paparazzi/omap/overo-oe/tmp/sysroots/armv7a-angstrom-linux-gnueabi/usr/include/eigen2 + test1.CXXFLAGS += $(LIB_EIGEN_CFLAGS) test1.cpp_srcs = fms/libeknav/hello_world.cpp + # test 2: now build with libeknav + test2.ARCHDIR = omap + test2.CXXFLAGS += $(LIB_EIGEN_CFLAGS) + test2.cpp_srcs = fms/libeknav/test_libeknav_2.cpp + test2.CXXFLAGS += $(LIBEKNAV_CFLAGS) + test2.cpp_srcs += $(LIBEKNAV_SRCS) + + # test 3: now try to add Paparazzi's C + test3.ARCHDIR = omap + test3.CXXFLAGS += $(LIB_EIGEN_CFLAGS) + test3.CXXFLAGS += -I$(PAPARAZZI_HOME)/var/$(AIRCRAFT) + test3.CXXFLAGS += -I$(PAPARAZZI_SRC)/sw/airborne + test3.CXXFLAGS += -I$(PAPARAZZI_SRC)/sw/include + test3.cpp_srcs = fms/libeknav/test_libeknav_3.cpp + test3.CXXFLAGS += $(LIBEKNAV_CFLAGS) + test3.cpp_srcs += $(LIBEKNAV_SRCS) + test3.LDFLAGS += -levent -lm + test3.CFLAGS += -DFMS_PERIODIC_FREQ=512 + test3.srcs += fms/fms_periodic.c + test3.CXXFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessagePTUp -DOVERO_LINK_MSG_DOWN=AutopilotMessagePTDown + test3.srcs += fms/fms_spi_link.c + # test network based telemetry on overo (using udp_transport2/messages2) overo_test_telemetry2.ARCHDIR = omap overo_test_telemetry2.CFLAGS += -I$(ACINCLUDE) -I. -I$(PAPARAZZI_HOME)/var/include diff --git a/sw/airborne/fms/libeknav/assertions.hpp b/sw/airborne/fms/libeknav/assertions.hpp new file mode 100644 index 0000000000..1af924abc5 --- /dev/null +++ b/sw/airborne/fms/libeknav/assertions.hpp @@ -0,0 +1,65 @@ +/* + * assertions.hpp + * + * Created on: Aug 6, 2009 + * Author: Jonathan Brandmeyer + * + * This file is part of libeknav. + * + * Libeknav 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, version 3. + * + * Libeknav 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 libeknav. If not, see . + * + */ + +#ifndef AHRS_ASSERTIONS_HPP +#define AHRS_ASSERTIONS_HPP + +#include +#include + +template +bool hasNaN(const MatrixBase& expr); + +template +bool hasInf(const MatrixBase& expr); + +template +bool hasNaN(const MatrixBase& expr) +{ + for (int j = 0; j != expr.cols(); ++j) { + for (int i = 0; i != expr.rows(); ++i) { + if (std::isnan(expr.coeff(i, j))) + return true; + } + } + return false; +} + +template +bool hasInf(const MatrixBase& expr) +{ + for (int i = 0; i != expr.cols(); ++i) { + for (int j = 0; j != expr.rows(); ++j) { + if (std::isinf(expr.coeff(j, i))) + return true; + } + } + return false; +} + +template +bool isReal(const MatrixBase& exp) +{ + return !hasNaN(exp) && ! hasInf(exp); +} + +#endif /* ASSERTIONS_HPP_ */ diff --git a/sw/airborne/fms/libeknav/hello_world.c b/sw/airborne/fms/libeknav/hello_world.c deleted file mode 100644 index 141dbb797b..0000000000 --- a/sw/airborne/fms/libeknav/hello_world.c +++ /dev/null @@ -1,12 +0,0 @@ - - -#include - - -int main(int argc, char** argv) { - - printf("hello world\n"); - - return 0; -} - diff --git a/sw/airborne/fms/libeknav/ins_qkf.hpp b/sw/airborne/fms/libeknav/ins_qkf.hpp index ef368f0d53..9f1edec48d 100644 --- a/sw/airborne/fms/libeknav/ins_qkf.hpp +++ b/sw/airborne/fms/libeknav/ins_qkf.hpp @@ -20,7 +20,7 @@ * along with libeknav. If not, see . */ -#include "sigma_points.hpp" +//#include "sigma_points.hpp" #include "quaternions.hpp" #include diff --git a/sw/airborne/fms/libeknav/ins_qkf_observe_gps_p.cpp b/sw/airborne/fms/libeknav/ins_qkf_observe_gps_p.cpp new file mode 100644 index 0000000000..3a27293e78 --- /dev/null +++ b/sw/airborne/fms/libeknav/ins_qkf_observe_gps_p.cpp @@ -0,0 +1,64 @@ +/* + * ins_qkf_observe_gps_p.cpp + * + * Created on: Jun 10, 2010 + * Author: Jonathan Brandmeyer + * + * This file is part of libeknav. + * + * Libeknav 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, version 3. + * + * Libeknav 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 libeknav. If not, see . + * + */ + +#include "ins_qkf.hpp" +#include "assertions.hpp" + +#define RANK_ONE_UPDATES + +#ifndef RANK_ONE_UPDATES +#include "timer.hpp" +#include +#endif + +using namespace Eigen; + +void +basic_ins_qkf::obs_gps_p_report(const Vector3d& pos, const Vector3d& p_error) +{ + Matrix residual = pos - avg_state.position; + std::cout << "diff_p(" < update = Matrix::Zero(); + for (int i = 0; i < 3; ++i) { + double innovation_cov_inv = 1.0/(cov(6+i, 6+i) + p_error[i]); + Matrix gain = cov.block<12, 1>(0, 6+i) * innovation_cov_inv; + update += gain * (residual[i] - update[6+i]); + cov -= gain * cov.block<1, 12>(6+i, 0); + } + +#else + Matrix innovation_cov = cov.block<3, 3>(6, 6); + innovation_cov += p_error.asDiagonal(); + + Matrix kalman_gain = cov.block<12, 3>(0, 6) + * innovation_cov.part().inverse(); + Matrix update = kalman_gain * residual; + cov.part() -= kalman_gain * cov.block<3, 12>(6, 0); +#endif + Quaterniond rotor = avg_state.apply_kalman_vec_update(update); + std::cout << "update(" << update.segment<6>(0).transpose()*180/M_PI << "\t" << update.segment<6>(6).transpose() << ")\n"; + counter_rotate_cov(rotor); + assert(is_real()); +} diff --git a/sw/airborne/fms/libeknav/ins_qkf_observe_gps_pvt.cpp b/sw/airborne/fms/libeknav/ins_qkf_observe_gps_pvt.cpp index 5040ecc5da..9ee97fbbd0 100644 --- a/sw/airborne/fms/libeknav/ins_qkf_observe_gps_pvt.cpp +++ b/sw/airborne/fms/libeknav/ins_qkf_observe_gps_pvt.cpp @@ -22,9 +22,9 @@ #include "ins_qkf.hpp" #include "assertions.hpp" #include -#include "timer.hpp" #ifdef TIME_OPS +#include "timer.hpp" #include #endif diff --git a/sw/airborne/fms/libeknav/ins_qkf_observe_vector.cpp b/sw/airborne/fms/libeknav/ins_qkf_observe_vector.cpp index aef4066e24..4396c4fa4d 100644 --- a/sw/airborne/fms/libeknav/ins_qkf_observe_vector.cpp +++ b/sw/airborne/fms/libeknav/ins_qkf_observe_vector.cpp @@ -21,15 +21,17 @@ #include "ins_qkf.hpp" #include "assertions.hpp" -#include "timer.hpp" #ifdef TIME_OPS +#include "timer.hpp" #include #endif using namespace Eigen; #define RANK_ONE_UPDATES +#include + void basic_ins_qkf::obs_gyro_bias(const Vector3d& bias, const Vector3d& bias_error) { diff --git a/sw/airborne/fms/libeknav/ins_qkf_predict.cpp b/sw/airborne/fms/libeknav/ins_qkf_predict.cpp index 7ac1f97172..83b830dc50 100644 --- a/sw/airborne/fms/libeknav/ins_qkf_predict.cpp +++ b/sw/airborne/fms/libeknav/ins_qkf_predict.cpp @@ -21,9 +21,9 @@ #include "ins_qkf.hpp" #include "assertions.hpp" -#include "timer.hpp" #ifdef TIME_OPS +#include "timer.hpp" # include #endif diff --git a/sw/airborne/fms/libeknav/test_libeknav_1.cpp b/sw/airborne/fms/libeknav/test_libeknav_1.cpp new file mode 100644 index 0000000000..90be903911 --- /dev/null +++ b/sw/airborne/fms/libeknav/test_libeknav_1.cpp @@ -0,0 +1,21 @@ + + +#include +#include + +#include + +// import most common Eigen types +USING_PART_OF_NAMESPACE_EIGEN + +int main(int, char *[]) { + + std::cout << "hello world" << std::endl; + + Matrix3f m3; + m3 << 1, 2, 3, 4, 5, 6, 7, 8, 9; + + return 0; + +} + diff --git a/sw/airborne/fms/libeknav/test_libeknav_2.cpp b/sw/airborne/fms/libeknav/test_libeknav_2.cpp new file mode 100644 index 0000000000..b9305df183 --- /dev/null +++ b/sw/airborne/fms/libeknav/test_libeknav_2.cpp @@ -0,0 +1,62 @@ + + +#include +#include + +#include + +#include "ins_qkf.hpp" + +//#include "std.h" +#define RadOfDeg(x) ((x) * (M_PI/180.)) + +// import most common Eigen types +USING_PART_OF_NAMESPACE_EIGEN + +int main(int, char *[]) { + + std::cout << "test libeknav 1" << std::endl; + + /* initial state */ + Vector3d pos_0_ecef(1017.67e3, -5079.282e3, 3709.041e3); + Vector3d speed_0_ecef(0., 0., 0.); + // Vector3d orientation(0., 0., 0.); + Vector3d bias_0(0., 0., 0.); + + /* initial covariance */ + const double pos_cov_0 = 1e2*1e2; + const double speed_cov_0 = 3.*3.; + const double orientation_cov_0 = RadOfDeg(5.)*RadOfDeg(5.); + const double bias_cov_0 = 0.447; + + /* system noise */ + const Vector3d gyro_white_noise = Vector3d::Ones()*0.1*0.1; + const Vector3d gyro_stability_noise = Vector3d::Ones()*0.00001; + const Vector3d accel_white_noise = Vector3d::Ones()* 0.04*0.04; + + /* measurement noise */ + const double mag_noise = std::pow(5 / 180.0 * M_PI, 2); + const Vector3d gps_pos_noise = Vector3d::Ones() *10*10; + const Vector3d gps_speed_noise = Vector3d::Ones()*0.1*0.1; + + /* sensors */ + Vector3d gyro(0., 0., 0.); + Vector3d accelerometer(0., 0., 9.81); + Vector3d magnetometer = Vector3d::UnitZ(); + + basic_ins_qkf ins(pos_0_ecef, pos_cov_0, bias_cov_0, speed_cov_0, + gyro_white_noise, gyro_stability_noise, accel_white_noise); + + const double dt = 1./512.; /* predict at 512Hz */ + for (int i=1; i<5000; i++) { + ins.predict(gyro, accelerometer, dt); + if (i % 10 == 0) /* update mag at 50Hz */ + ins.obs_vector(magnetometer, magnetometer, mag_noise); + if (i % 128 == 0) /* update gps at 4 Hz */ + ins.obs_gps_pv_report(pos_0_ecef, speed_0_ecef, gps_pos_noise, gps_speed_noise); + } + + return 0; + +} + diff --git a/sw/airborne/fms/libeknav/test_libeknav_3.cpp b/sw/airborne/fms/libeknav/test_libeknav_3.cpp new file mode 100644 index 0000000000..b21486518b --- /dev/null +++ b/sw/airborne/fms/libeknav/test_libeknav_3.cpp @@ -0,0 +1,130 @@ + +#include +#include + +#include + +#include "ins_qkf.hpp" + + +#include +extern "C" { +#include "std.h" +#include "fms/fms_debug.h" +#include "fms/fms_periodic.h" +#include "fms/fms_spi_link.h" +#include "fms/fms_autopilot_msg.h" +#include "booz/booz_imu.h" + /* our sensors */ + struct BoozImuFloat imu; +} + +static void main_init(void); +static void main_periodic(int my_sig_num); +static void main_dialog_with_io_proc(void); + + +/* initial state */ +Vector3d pos_0_ecef(1017.67e3, -5079.282e3, 3709.041e3); +Vector3d speed_0_ecef(0., 0., 0.); +// Vector3d orientation(0., 0., 0.); +Vector3d bias_0(0., 0., 0.); + +/* initial covariance */ +const double pos_cov_0 = 1e2*1e2; +const double speed_cov_0 = 3.*3.; +// const double orientation_cov_0 = RadOfDeg(5.)*RadOfDeg(5.); +const double bias_cov_0 = 0.447; + +/* system noise */ +const Vector3d gyro_white_noise = Vector3d::Ones()*0.1*0.1; +const Vector3d gyro_stability_noise = Vector3d::Ones()*0.00001; +const Vector3d accel_white_noise = Vector3d::Ones()* 0.04*0.04; + +static basic_ins_qkf ins = basic_ins_qkf(pos_0_ecef, pos_cov_0, bias_cov_0, speed_cov_0, + gyro_white_noise, gyro_stability_noise, accel_white_noise); + + +// import most common Eigen types +USING_PART_OF_NAMESPACE_EIGEN + +int main(int, char *[]) { + + main_init(); + + std::cout << "test libeknav 1" << std::endl; + + /* Enter our mainloop */ + event_dispatch(); + + TRACE(TRACE_DEBUG, "%s", "leaving mainloop... goodbye!\n"); + + return 0; + +} + + +static void main_init(void) { + + TRACE(TRACE_DEBUG, "%s", "Starting initialization\n"); + + /* Initalize our SPI link to IO processor */ + if (spi_link_init()) { + TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); + return; + } + + /* Initalize the event library */ + event_init(); + + /* Initalize our ô so accurate periodic timer */ + if (fms_periodic_init(main_periodic)) { + TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); + return; + } + +} + + +static void main_periodic(int my_sig_num __attribute__ ((unused))) { + + static uint32_t cnt; + cnt++; + + main_dialog_with_io_proc(); + + const double dt = 1./512.; + Vector3d gyro(0., 0., 0.); + Vector3d accelerometer(0., 0., 9.81); + ins.predict(gyro, accelerometer, dt); + + if (cnt % 10 == 0) { /* update mag at 50Hz */ + Vector3d magnetometer = Vector3d::UnitZ(); + const double mag_noise = std::pow(5 / 180.0 * M_PI, 2); + ins.obs_vector(magnetometer, magnetometer, mag_noise); + } + if (cnt % 128 == 0) /* update gps at 4 Hz */ { + 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(pos_0_ecef, speed_0_ecef, gps_pos_noise, gps_speed_noise); + } + +} + + +static void main_dialog_with_io_proc() { + + struct AutopilotMessageCRCFrame msg_in; + struct AutopilotMessageCRCFrame msg_out; + 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); + + struct AutopilotMessagePTUp *in = &msg_in.payload.msg_up; + RATES_FLOAT_OF_BFP(imu.gyro, in->gyro); + ACCELS_FLOAT_OF_BFP(imu.accel, in->accel); + MAGS_FLOAT_OF_BFP(imu.mag, in->mag); + +}