diff --git a/boards/holybro/durandal-v1/stackcheck.cmake b/boards/holybro/durandal-v1/stackcheck.cmake index 910f57a4b3..4a77901592 100644 --- a/boards/holybro/durandal-v1/stackcheck.cmake +++ b/boards/holybro/durandal-v1/stackcheck.cmake @@ -107,13 +107,13 @@ px4_add_board( ver work_queue EXAMPLES - bottle_drop # OBC challenge - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test + # bottle_drop # OBC challenge + # fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + # hello + # hwtest # Hardware test #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app + # px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + # px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + # rover_steering_control # Rover example app + # uuv_example_app ) diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake index 4274b208e2..4d5e9a00fd 100644 --- a/boards/px4/fmu-v2/fixedwing.cmake +++ b/boards/px4/fmu-v2/fixedwing.cmake @@ -23,9 +23,9 @@ px4_add_board( adc #barometer # all available barometer drivers barometer/ms5611 - batt_smbus - camera_capture - camera_trigger + # batt_smbus + # camera_capture + # camera_trigger differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers gps @@ -48,7 +48,7 @@ px4_add_board( commander dataman ekf2 - events + # events fw_att_control fw_pos_control_l1 land_detector @@ -67,7 +67,7 @@ px4_add_board( #dumpfile #esc_calib hardfault_log - i2cdetect + # i2cdetect #led_control mixer #motor_ramp @@ -82,8 +82,8 @@ px4_add_board( top #topic_listener tune_control - usb_connected + # usb_connected ver - work_queue + # work_queue ) diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index 31318ecd5c..fd3a8d2b81 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -22,8 +22,8 @@ px4_add_board( adc barometer/ms5611 #batt_smbus - camera_capture - camera_trigger + # camera_capture + # camera_trigger distance_sensor # all available distance sensor drivers gps imu/l3gd20 @@ -80,7 +80,7 @@ px4_add_board( top #topic_listener tune_control - usb_connected + # usb_connected ver #work_queue diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index 69c5dade1a..ff47587f68 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -60,7 +60,7 @@ px4_add_board( MODULES #attitude_estimator_q - camera_feedback + # camera_feedback commander dataman #ekf2 @@ -68,8 +68,8 @@ px4_add_board( #fw_att_control #fw_pos_control_l1 #rover_pos_control - land_detector - landing_target_estimator + # land_detector + # landing_target_estimator load_mon #local_position_estimator logger @@ -81,7 +81,7 @@ px4_add_board( battery_status sensors vmount - vtol_att_control + # vtol_att_control #airspeed_selector SYSTEMCMDS diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index a42f57288a..a2f9181299 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -106,7 +106,7 @@ px4_add_board( reflect sd_bench shutdown - tests # tests and test runner + # tests # tests and test runner top topic_listener tune_control diff --git a/src/lib/matrix b/src/lib/matrix index cd185c995b..a172c3cdac 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit cd185c995b6b28c12e06d20a2743a94bd68be7c2 +Subproject commit a172c3cdac9260369fb5805fcce19067121566e5 diff --git a/src/systemcmds/tests/test_matrix.cpp b/src/systemcmds/tests/test_matrix.cpp index a3c4d0e3ff..bd95e468b7 100644 --- a/src/systemcmds/tests/test_matrix.cpp +++ b/src/systemcmds/tests/test_matrix.cpp @@ -68,6 +68,7 @@ private: bool vector3Tests(); bool vectorAssignmentTests(); bool dcmRenormTests(); + bool pseudoInverseTests(); }; bool MatrixTest::run_tests() @@ -89,6 +90,7 @@ bool MatrixTest::run_tests() ut_run_test(vector3Tests); ut_run_test(vectorAssignmentTests); ut_run_test(dcmRenormTests); + ut_run_test(pseudoInverseTests); return (_tests_failed == 0); } @@ -757,3 +759,98 @@ bool MatrixTest::dcmRenormTests() return true; } + +bool MatrixTest::pseudoInverseTests() +{ + // 3x4 Matrix test + float data0[12] = { + 0.f, 1.f, 2.f, 3.f, + 4.f, 5.f, 6.f, 7.f, + 8.f, 9.f, 10.f, 11.f + }; + + float data0_check[12] = { + -0.3375f, -0.1f, 0.1375f, + -0.13333333f, -0.03333333f, 0.06666667f, + 0.07083333f, 0.03333333f, -0.00416667f, + 0.275f, 0.1f, -0.075f + }; + + Matrix A0(data0); + Matrix A0_I = geninv(A0); + Matrix A0_I_check(data0_check); + + ut_test((A0_I - A0_I_check).abs().max() < 1e-5); + + // 4x3 Matrix test + float data1[12] = { + 0.f, 4.f, 8.f, + 1.f, 5.f, 9.f, + 2.f, 6.f, 10.f, + 3.f, 7.f, 11.f + }; + + float data1_check[12] = { + -0.3375f, -0.13333333f, 0.07083333f, 0.275f, + -0.1f, -0.03333333f, 0.03333333f, 0.1f, + 0.1375f, 0.06666667f, -0.00416667f, -0.075f + }; + + Matrix A1(data1); + Matrix A1_I = geninv(A1); + Matrix A1_I_check(data1_check); + + ut_test((A1_I - A1_I_check).abs().max() < 1e-5); + + // Square matrix test + float data2[9] = { + 0, 2, 3, + 4, 5, 6, + 7, 8, 10 + }; + float data2_check[9] = { + -0.4f, -0.8f, 0.6f, + -0.4f, 4.2f, -2.4f, + 0.6f, -2.8f, 1.6f + }; + + SquareMatrix A2(data2); + SquareMatrix A2_I = inv(A2); + SquareMatrix A2_I_check(data2_check); + ut_test((A2_I - A2_I_check).abs().max() < 1e-5); + + // Mock-up effectiveness matrix + const float B_quad_w[6][16] = { + {-0.5717536f, 0.43756646f, 0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.35355328f, -0.35355328f, 0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + Matrix B(B_quad_w); + const float A_quad_w[16][6] = { + { -0.495383f, 0.707107f, 0.765306f, 0.0f, 0.0f, -1.000000f }, + { 0.495383f, -0.707107f, 1.000000f, 0.0f, 0.0f, -1.000000f }, + { 0.495383f, 0.707107f, -0.765306f, 0.0f, 0.0f, -1.000000f }, + { -0.495383f, -0.707107f, -1.000000f, 0.0f, 0.0f, -1.000000f }, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f} + }; + Matrix A_check(A_quad_w); + Matrix A = geninv(B); + ut_test((A - A_check).abs().max() < 1e-5); + + return true; + +} diff --git a/src/systemcmds/tests/test_microbench_matrix.cpp b/src/systemcmds/tests/test_microbench_matrix.cpp index dd1d8183a8..86c00fc8b8 100644 --- a/src/systemcmds/tests/test_microbench_matrix.cpp +++ b/src/systemcmds/tests/test_microbench_matrix.cpp @@ -101,7 +101,9 @@ private: matrix::Quatf q; matrix::Eulerf e; matrix::Dcmf d; - + matrix::Matrix A16; + matrix::Matrix B16; + matrix::Matrix B16_4; }; bool MicroBenchMatrix::run_tests() @@ -126,6 +128,17 @@ void MicroBenchMatrix::reset() q = matrix::Quatf(rand(), rand(), rand(), rand()); e = matrix::Eulerf(random(-2.0 * M_PI, 2.0 * M_PI), random(-2.0 * M_PI, 2.0 * M_PI), random(-2.0 * M_PI, 2.0 * M_PI)); d = q; + + for (size_t j = 0; j < 6; j++) { + for (size_t i = 0; i < 16; i++) { + B16(j, i) = random(-10.0, 10.0); + } + + for (size_t i = 0; i < 4; i++) { + B16_4(j, i) = random(-10.0, 10.0); + } + } + } ut_declare_test_c(test_microbench_matrix, MicroBenchMatrix) @@ -141,6 +154,9 @@ bool MicroBenchMatrix::time_px4_matrix() PERF("matrix Dcm from Euler", d = e, 1000); PERF("matrix Dcm from Quaternion", d = q, 1000); + PERF("matrix 6x16 pseudo inverse (all non-zero columns)", A16 = matrix::geninv(B16), 1000); + PERF("matrix 6x16 pseudo inverse (4 non-zero columns)", A16 = matrix::geninv(B16_4), 1000); + return true; }