fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update matrix submodule and add pseudoinverse tests (tests matrix)

Browse Source
This commit is contained in:
Julien Lecoeur 2019-11-21 19:06:59 +01:00 committed by Daniel Agar
parent 5d3a6d8213
commit a46581987c
8 changed files with 138 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
boards/holybro/durandal-v1/stackcheck.cmake
View File

@ -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
)

14
boards/px4/fmu-v2/fixedwing.cmake
View File

@ -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
)

6
boards/px4/fmu-v2/multicopter.cmake
View File

@ -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

8
boards/px4/fmu-v2/test.cmake
View File

@ -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

2
boards/px4/fmu-v5/stackcheck.cmake
View File

@ -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

2
src/lib/matrix

@ -1 +1 @@
Subproject commit cd185c995b6b28c12e06d20a2743a94bd68be7c2
Subproject commit a172c3cdac9260369fb5805fcce19067121566e5

97
src/systemcmds/tests/test_matrix.cpp
View File

@ -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<float, 3, 4> A0(data0);
Matrix<float, 4, 3> A0_I = geninv(A0);
Matrix<float, 4, 3> 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<float, 4, 3> A1(data1);
Matrix<float, 3, 4> A1_I = geninv(A1);
Matrix<float, 3, 4> 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<float, 3> A2(data2);
SquareMatrix<float, 3> A2_I = inv(A2);
SquareMatrix<float, 3> 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<float, 6, 16> 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<float, 16, 6> A_check(A_quad_w);
Matrix<float, 16, 6> A = geninv(B);
ut_test((A - A_check).abs().max() < 1e-5);
return true;
}

18
src/systemcmds/tests/test_microbench_matrix.cpp
View File

@ -101,7 +101,9 @@ private:
matrix::Quatf q;
matrix::Eulerf e;
matrix::Dcmf d;
matrix::Matrix<float, 16, 6> A16;
matrix::Matrix<float, 6, 16> B16;
matrix::Matrix<float, 6, 16> 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;
}