mathlib delete Matrix, Quaternion, Vector

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -45,6 +45,7 @@
 #include <lib/ecl/geo_lookup/geo_mag_declination.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
 #include <px4_config.h>
 #include <px4_posix.h>
 #include <px4_tasks.h>

src/modules/commander/calibration_routines.cpp

@@ -51,6 +51,7 @@
 #include <lib/ecl/geo/geo.h>
 #include <string.h>
 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
 
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/sensor_combined.h>
@@ -270,13 +271,10 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
 	float fitness = _fitness;
 	float fit1 = 0.0f, fit2 = 0.0f;
 
-	float JTJ[16];
-	float JTJ2[16];
-	float JTFI[4];
+	matrix::SquareMatrix<float, 4> JTJ;
+	matrix::SquareMatrix<float, 4> JTJ2;
+	float JTFI[4] = {};
 	float residual = 0.0f;
-	memset(JTJ, 0, sizeof(JTJ));
-	memset(JTJ2, 0, sizeof(JTJ2));
-	memset(JTFI, 0, sizeof(JTFI));
 
 	// Gauss Newton Part common for all kind of extensions including LM
 	for (uint16_t k = 0; k < _samples_collected; k++) {
@@ -299,8 +297,8 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
 		for (uint8_t i = 0; i < 4; i++) {
 			// compute JTJ
 			for (uint8_t j = 0; j < 4; j++) {
-				JTJ[i * 4 + j] += sphere_jacob[i] * sphere_jacob[j];
-				JTJ2[i * 4 + j] += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM
+				JTJ(i, j) += sphere_jacob[i] * sphere_jacob[j];
+				JTJ2(i, j) += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM
 			}
 
 			JTFI[i] += sphere_jacob[i] * residual;
@@ -315,22 +313,22 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
 	memcpy(fit2_params, fit1_params, sizeof(fit1_params));
 
 	for (uint8_t i = 0; i < 4; i++) {
-		JTJ[i * 4 + i] += _sphere_lambda;
-		JTJ2[i * 4 + i] += _sphere_lambda / lma_damping;
+		JTJ(i, i) += _sphere_lambda;
+		JTJ2(i, i) += _sphere_lambda / lma_damping;
 	}
 
-	if (!inverse4x4(JTJ, JTJ)) {
+	if (!JTJ.I(JTJ)) {
 		return -1;
 	}
 
-	if (!inverse4x4(JTJ2, JTJ2)) {
+	if (!JTJ2.I(JTJ2)) {
 		return -1;
 	}
 
 	for (uint8_t row = 0; row < 4; row++) {
 		for (uint8_t col = 0; col < 4; col++) {
-			fit1_params[row] -= JTFI[col] * JTJ[row * 4 + col];
-			fit2_params[row] -= JTFI[col] * JTJ2[row * 4 + col];
+			fit1_params[row] -= JTFI[col] * JTJ(row, col);
+			fit2_params[row] -= JTFI[col] * JTJ2(row, col);
 		}
 	}
 
@@ -395,15 +393,13 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
 	const float lma_damping = 10.0f;
 	float _samples_collected = size;
 	float fitness = _fitness;
-	float fit1 = 0.0f, fit2 = 0.0f;
+	float fit1 = 0.0f;
+	float fit2 = 0.0f;
 
-	float JTJ[81];
-	float JTJ2[81];
-	float JTFI[9];
+	float JTJ[81] = {};
+	float JTJ2[81] = {};
+	float JTFI[9] = {};
 	float residual = 0.0f;
-	memset(JTJ, 0, sizeof(JTJ));
-	memset(JTJ2, 0, sizeof(JTJ2));
-	memset(JTFI, 0, sizeof(JTFI));
 	float ellipsoid_jacob[9];
 
 	// Gauss Newton Part common for all kind of extensions including LM
@@ -461,8 +457,6 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
 		return -1;
 	}
 
-
-
 	for (uint8_t row = 0; row < 9; row++) {
 		for (uint8_t col = 0; col < 9; col++) {
 			fit1_params[row] -= JTFI[col] * JTJ[row * 9 + col];

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -39,6 +39,7 @@
 #include <ecl/attitude_fw/ecl_yaw_controller.h>
 #include <lib/ecl/geo/geo.h>
 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <px4_posix.h>

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h

@@ -49,6 +49,7 @@
 #include <px4_module_params.h>
 #include <systemlib/mavlink_log.h>
 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
 
 namespace runwaytakeoff
 {

src/modules/gnd_att_control/CMakeLists.txt

@@ -37,5 +37,4 @@ px4_add_module(
 	COMPILE_FLAGS
 	SRCS
 		GroundRoverAttitudeControl.cpp
-	DEPENDS
 	)

src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp

@@ -48,6 +48,7 @@
 
 #include <drivers/drv_hrt.h>
 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
 #include <parameters/param.h>
 #include <systemlib/pid/pid.h>
 #include <perf/perf_counter.h>

src/modules/logger/log_writer_mavlink.cpp

@@ -38,6 +38,7 @@
 #include <mathlib/mathlib.h>
 #include <px4_log.h>
 #include <px4_posix.h>
+#include <cstring>
 
 namespace px4
 {

src/modules/mavlink/mavlink_messages.cpp

@@ -55,6 +55,7 @@
 #include <drivers/drv_rc_input.h>
 #include <lib/ecl/geo/geo.h>
 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
 #include <px4_time.h>
 #include <systemlib/err.h>
 #include <systemlib/mavlink_log.h>

src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp

@@ -42,6 +42,7 @@
 #include <systemlib/err.h>
 #include <unit_test.h>
 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
 
 extern "C" __EXPORT int mc_pos_control_tests_main(int argc, char *argv[]);
 

src/modules/navigator/follow_target.h

@@ -40,11 +40,12 @@
 
 #pragma once
 
-#include <mathlib/mathlib.h>
-
 #include "navigator_mode.h"
 #include "mission_block.h"
 
+#include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
+
 #include <px4_module_params.h>
 #include <uORB/topics/follow_target.h>
 

src/modules/sensors/temperature_compensation.h

@@ -44,6 +44,7 @@
 
 #include <parameters/param.h>
 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
 
 #include "common.h"
 

src/modules/vtol_att_control/tailsitter.cpp

@@ -242,9 +242,9 @@ void Tailsitter::update_transition_state()
 	_v_att_sp->roll_body = 0.0f;
 	_v_att_sp->yaw_body = _yaw_transition;
 
-	math::Quaternion q_sp;
-	q_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body);
-	memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d));
+	matrix::Quatf q_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body);
+	q_sp.copyTo(_v_att_sp->q_d);
+	_v_att_sp->q_d_valid = true;
 }
 
 void Tailsitter::waiting_on_tecs()

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -58,7 +58,8 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
 #include <lib/ecl/geo/geo.h>
-#include <lib/mathlib/mathlib.h>
+#include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
 #include <parameters/param.h>
 
 #include <uORB/topics/actuator_controls.h>

src/modules/wind_estimator/wind_estimator_main.cpp

@@ -32,8 +32,8 @@
  ****************************************************************************/
 
 #include <drivers/drv_hrt.h>
+#include <matrix/math.hpp>
 #include <ecl/airdata/WindEstimator.hpp>
-#include <matrix/matrix/math.hpp>
 #include <px4_module.h>
 #include <px4_module_params.h>
 #include <px4_workqueue.h>