introduce ecl_float_t type and use in EKF

CMakeLists.txt

@@ -90,19 +90,29 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
 	elseif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX)
 		add_compile_options(
 			-pedantic
-		
+
 			-Wall
 			-Wextra
 			-Werror
 
 			-Wno-missing-field-initializers # ignore for GCC 4.8 support
 		)
+
+		# enable color output
+		if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.9)
+			# force color for gcc > 4.9
+			add_compile_options(-fdiagnostics-color=always)
+		endif()
+
+	elseif ("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang")
+		# force color for clang (needed for clang + ccache)
+		add_compile_options(-fcolor-diagnostics)
 	endif()
 
 	# testing
 	include(CTest)
 	enable_testing()
-	
+
 	if(BUILD_TESTING)
 		option(ECL_TESTS "Build ECL tests" ON)
 
@@ -196,17 +206,17 @@ endif()
 if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
 
 	option(BUILD_DOXYGEN "Build doxygen documentation" OFF)
-	
+
 	if (BUILD_DOXYGEN)
 		find_package(Doxygen)
 		if (DOXYGEN_FOUND)
 			# set input and output files
 			set(DOXYGEN_IN ${CMAKE_CURRENT_SOURCE_DIR}/docs/Doxyfile.in)
 			set(DOXYGEN_OUT ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile)
-	
+
 			# request to configure the file
 			configure_file(${DOXYGEN_IN} ${DOXYGEN_OUT} @ONLY)
-	
+
 			# note the option ALL which allows to build the docs together with the application
 			add_custom_target(doxygen ALL
 				COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_OUT}
@@ -216,7 +226,7 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
 				VERBATIM
 				USES_TERMINAL
 			)
-	
+
 		else()
 			message(FATAL_ERROR "Doxygen needs to be installed to generate documentation")
 		endif()

EKF/CMakeLists.txt

@@ -31,6 +31,20 @@
 #
 ############################################################################
 
+if((${CONFIG_ARCH_DPFPU} MATCHES "y") AND ECL_USE_DOUBLE_FPU) # DISABLED for now
+	message(STATUS "PX4 ECL: using double precision floating point (ecl_float_t = double)")
+	add_definitions(-DECL_FLOAT_TYPE=double)
+
+	add_compile_options(
+		-Wno-double-promotion
+		-Wno-narrowing
+		-Wno-absolute-value
+	)
+else()
+	# ONLY in ecl_float_t = float32 mode
+	add_compile_options(-fsingle-precision-constant)
+endif()
+
 add_library(ecl_EKF
 	airspeed_fusion.cpp
 	control.cpp

EKF/airspeed_fusion.cpp

@@ -40,10 +40,15 @@
  * @author Paul Riseborough <[email protected]>
  *
  */
-#include "../ecl.h"
+
+#include <ecl.h>
+
 #include "ekf.h"
 #include <mathlib/mathlib.h>
 
+namespace estimator
+{
+
 void Ekf::fuseAirspeed()
 {
 	// Initialize variables
@@ -53,8 +58,7 @@ void Ekf::fuseAirspeed()
 	float vwn; // Wind speed in north direction
 	float vwe; // Wind speed in east direction
 	float v_tas_pred; // Predicted measurement
-	float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f,
-			 10.0f)); // Variance for true airspeed measurement - (m/sec)^2
+	float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); // Variance for true airspeed measurement - (m/sec)^2
 	float SH_TAS[3] = {}; // Variable used to optimise calculations of measurement jacobian
 	float H_TAS[24] = {}; // Observation Jacobian
 	float SK_TAS[2] = {}; // Variable used to optimise calculations of the Kalman gain vector
@@ -274,3 +278,5 @@ void Ekf::resetWindStates()
 		_state.wind_vel(1) = 0.0f;
 	}
 }
+
+} // namespace estimator

EKF/common.h

@@ -31,6 +31,8 @@
  *
  ****************************************************************************/
 
+#pragma once
+
 /**
  * @file common.h
  * Definition of base class for attitude estimators
@@ -45,13 +47,14 @@
 namespace estimator
 {
 
-using matrix::AxisAnglef;
-using matrix::Dcmf;
-using matrix::Eulerf;
-using matrix::Matrix3f;
-using matrix::Quatf;
-using matrix::Vector2f;
-using matrix::Vector3f;
+using AxisAnglef = matrix::AxisAngle<ecl_float_t>;
+using Dcmf = matrix::Dcm<ecl_float_t>;
+using Eulerf = matrix::Euler<ecl_float_t>;
+using Matrix3f = matrix::Matrix<ecl_float_t, 3, 3>;
+using Quatf = matrix::Quaternion<ecl_float_t>;
+using Vector2f = matrix::Vector2<ecl_float_t>;
+using Vector3f = matrix::Vector3<ecl_float_t>;
+
 using matrix::wrap_pi;
 
 struct gps_message {
@@ -102,11 +105,11 @@ struct outputVert {
 };
 
 struct imuSample {
-	Vector3f    delta_ang;		///< delta angle in body frame (integrated gyro measurements) (rad)
-	Vector3f    delta_vel;		///< delta velocity in body frame (integrated accelerometer measurements) (m/sec)
-	float       delta_ang_dt;	///< delta angle integration period (sec)
-	float       delta_vel_dt;	///< delta velocity integration period (sec)
-	uint64_t    time_us;		///< timestamp of the measurement (uSec)
+	Vector3f	delta_ang;	///< delta angle in body frame (integrated gyro measurements) (rad)
+	Vector3f	delta_vel;	///< delta velocity in body frame (integrated accelerometer measurements) (m/sec)
+	ecl_float_t	delta_ang_dt;	///< delta angle integration period (sec)
+	ecl_float_t	delta_vel_dt;	///< delta velocity integration period (sec)
+	uint64_t	time_us;	///< timestamp of the measurement (uSec)
 };
 
 struct gpsSample {
@@ -138,16 +141,16 @@ struct rangeSample {
 
 struct airspeedSample {
 	float       true_airspeed;	///< true airspeed measurement (m/sec)
-	float 		eas2tas;	///< equivalent to true airspeed factor
+	float       eas2tas;		///< equivalent to true airspeed factor
 	uint64_t    time_us;		///< timestamp of the measurement (uSec)
 };
 
 struct flowSample {
-	uint8_t  quality;	///< quality indicator between 0 and 255
-	Vector2f flowRadXY;	///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive
-	Vector3f gyroXYZ;	///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
-	float    dt;		///< amount of integration time (sec)
-	uint64_t time_us;	///< timestamp of the integration period leading edge (uSec)
+	Vector2f	flowRadXY;	///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive
+	Vector3f	gyroXYZ;	///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
+	ecl_float_t	dt;		///< amount of integration time (sec)
+	uint64_t	time_us;	///< timestamp of the integration period leading edge (uSec)
+	uint8_t		quality;	///< quality indicator between 0 and 255
 };
 
 struct extVisionSample {