forked from Archive/PX4-Autopilot
format src/systemcmds/tests
This commit is contained in:
parent
bed3fdf952
commit
2fdbdd15ec
|
@ -130,7 +130,8 @@ math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q);
|
||||||
* Construct new Eigen::Quaternion from euler angles
|
* Construct new Eigen::Quaternion from euler angles
|
||||||
* Right order is YPR.
|
* Right order is YPR.
|
||||||
**/
|
**/
|
||||||
Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy){
|
Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy)
|
||||||
|
{
|
||||||
return Eigen::Quaternionf(
|
return Eigen::Quaternionf(
|
||||||
Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) *
|
Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) *
|
||||||
Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) *
|
Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) *
|
||||||
|
@ -143,7 +144,8 @@ Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy){
|
||||||
* Construct new Eigen::Vector3f of euler angles from quaternion
|
* Construct new Eigen::Vector3f of euler angles from quaternion
|
||||||
* Right order is YPR.
|
* Right order is YPR.
|
||||||
**/
|
**/
|
||||||
Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q){
|
Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q)
|
||||||
|
{
|
||||||
return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
|
return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -151,7 +153,8 @@ Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q){
|
||||||
* @brief
|
* @brief
|
||||||
* Construct new Eigen::Matrix3f from euler angles
|
* Construct new Eigen::Matrix3f from euler angles
|
||||||
**/
|
**/
|
||||||
Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy){
|
Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy)
|
||||||
|
{
|
||||||
return quatFromEuler(rpy).toRotationMatrix();
|
return quatFromEuler(rpy).toRotationMatrix();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -159,7 +162,8 @@ Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy){
|
||||||
* @brief
|
* @brief
|
||||||
* Adjust PX4 math::quaternion to Eigen::Quaternionf
|
* Adjust PX4 math::quaternion to Eigen::Quaternionf
|
||||||
**/
|
**/
|
||||||
Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q){
|
Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q)
|
||||||
|
{
|
||||||
return Eigen::Quaternionf(q.data[1], q.data[2], q.data[3], q.data[0]);
|
return Eigen::Quaternionf(q.data[1], q.data[2], q.data[3], q.data[0]);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -167,7 +171,8 @@ Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q){
|
||||||
* @brief
|
* @brief
|
||||||
* Adjust Eigen::Quaternionf to PX4 math::quaternion
|
* Adjust Eigen::Quaternionf to PX4 math::quaternion
|
||||||
**/
|
**/
|
||||||
math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q){
|
math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q)
|
||||||
|
{
|
||||||
return math::Quaternion(q.w(), q.x(), q.y(), q.z());
|
return math::Quaternion(q.w(), q.x(), q.y(), q.z());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -175,7 +180,8 @@ math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q){
|
||||||
* @brief
|
* @brief
|
||||||
* Testing main routine
|
* Testing main routine
|
||||||
**/
|
**/
|
||||||
int test_eigen(int argc, char *argv[]) {
|
int test_eigen(int argc, char *argv[])
|
||||||
|
{
|
||||||
int rc = 0;
|
int rc = 0;
|
||||||
warnx("Testing Eigen math...");
|
warnx("Testing Eigen math...");
|
||||||
|
|
||||||
|
@ -362,6 +368,7 @@ int test_eigen(int argc, char *argv[]) {
|
||||||
++err_num;
|
++err_num;
|
||||||
rc = 1;
|
rc = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
/********************************************************************************************************/
|
/********************************************************************************************************/
|
||||||
/******************************************** TEST 2 ****************************************************/
|
/******************************************** TEST 2 ****************************************************/
|
||||||
q_true = {1.0f, 0.0f, 0.0f, 0.0f};
|
q_true = {1.0f, 0.0f, 0.0f, 0.0f};
|
||||||
|
@ -376,6 +383,7 @@ int test_eigen(int argc, char *argv[]) {
|
||||||
++err_num;
|
++err_num;
|
||||||
rc = 1;
|
rc = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
/********************************************************************************************************/
|
/********************************************************************************************************/
|
||||||
/******************************************** TEST 3 ****************************************************/
|
/******************************************** TEST 3 ****************************************************/
|
||||||
q_true = quatFromEuler(Eigen::Vector3f(0.3f, 0.2f, 0.1f));
|
q_true = quatFromEuler(Eigen::Vector3f(0.3f, 0.2f, 0.1f));
|
||||||
|
@ -388,6 +396,7 @@ int test_eigen(int argc, char *argv[]) {
|
||||||
++err_num;
|
++err_num;
|
||||||
rc = 1;
|
rc = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
/********************************************************************************************************/
|
/********************************************************************************************************/
|
||||||
/******************************************** TEST 4 ****************************************************/
|
/******************************************** TEST 4 ****************************************************/
|
||||||
q_true = quatFromEuler(Eigen::Vector3f(-1.5f, -0.2f, 0.5f));
|
q_true = quatFromEuler(Eigen::Vector3f(-1.5f, -0.2f, 0.5f));
|
||||||
|
@ -400,6 +409,7 @@ int test_eigen(int argc, char *argv[]) {
|
||||||
++err_num;
|
++err_num;
|
||||||
rc = 1;
|
rc = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
/********************************************************************************************************/
|
/********************************************************************************************************/
|
||||||
/******************************************** TEST 5 ****************************************************/
|
/******************************************** TEST 5 ****************************************************/
|
||||||
q_true = quatFromEuler(Eigen::Vector3f(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3));
|
q_true = quatFromEuler(Eigen::Vector3f(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3));
|
||||||
|
@ -407,13 +417,15 @@ int test_eigen(int argc, char *argv[]) {
|
||||||
|
|
||||||
for (size_t i = 0; i < 4; i++) {
|
for (size_t i = 0; i < 4; i++) {
|
||||||
if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
|
if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
|
||||||
printf("%llu: It[%d]: Value of: Quaternion [0.6830, 0.1830, -0.6830, 0.1830]\n", (unsigned long long)hrt_absolute_time(), (int)i);
|
printf("%llu: It[%d]: Value of: Quaternion [0.6830, 0.1830, -0.6830, 0.1830]\n",
|
||||||
|
(unsigned long long)hrt_absolute_time(), (int)i);
|
||||||
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
|
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
|
||||||
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
|
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
|
||||||
++err_num;
|
++err_num;
|
||||||
rc = 1;
|
rc = 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/********************************************************************************************************/
|
/********************************************************************************************************/
|
||||||
/******************************************** TEST 6 ****************************************************/
|
/******************************************** TEST 6 ****************************************************/
|
||||||
printf("%llu: Conversion method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
|
printf("%llu: Conversion method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
|
||||||
|
@ -430,7 +442,8 @@ int test_eigen(int argc, char *argv[]) {
|
||||||
for (size_t i = 0; i < 3; i++) {
|
for (size_t i = 0; i < 3; i++) {
|
||||||
for (size_t j = 0; j < 3; j++) {
|
for (size_t j = 0; j < 3; j++) {
|
||||||
if (!EXPECT_NEAR(R_orig(i, j), R(i, j), epsilon_f)) {
|
if (!EXPECT_NEAR(R_orig(i, j), R(i, j), epsilon_f)) {
|
||||||
printf("%llu: (%d, %d) Value of: Quaternion constructor or 'toRotationMatrix'\n", (unsigned long long)hrt_absolute_time(), (int)i, (int)j);
|
printf("%llu: (%d, %d) Value of: Quaternion constructor or 'toRotationMatrix'\n",
|
||||||
|
(unsigned long long)hrt_absolute_time(), (int)i, (int)j);
|
||||||
printf("Actual: \t%8.5f\n", R(i, j));
|
printf("Actual: \t%8.5f\n", R(i, j));
|
||||||
printf("Expected: \t%8.5f\n", R_orig(i, j));
|
printf("Expected: \t%8.5f\n", R_orig(i, j));
|
||||||
++err_num;
|
++err_num;
|
||||||
|
@ -468,6 +481,7 @@ int test_eigen(int argc, char *argv[]) {
|
||||||
rc = 1;
|
rc = 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/********************************************************************************************************/
|
/********************************************************************************************************/
|
||||||
/******************************************** TEST 2 ****************************************************/
|
/******************************************** TEST 2 ****************************************************/
|
||||||
q = quatFromEuler(Eigen::Vector3f(0.1f, 0.2f, 0.3f));
|
q = quatFromEuler(Eigen::Vector3f(0.1f, 0.2f, 0.3f));
|
||||||
|
@ -483,6 +497,7 @@ int test_eigen(int argc, char *argv[]) {
|
||||||
rc = 1;
|
rc = 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/********************************************************************************************************/
|
/********************************************************************************************************/
|
||||||
/******************************************** TEST 3 ****************************************************/
|
/******************************************** TEST 3 ****************************************************/
|
||||||
q = quatFromEuler(Eigen::Vector3f(0.5f, -0.2f, -1.5f));
|
q = quatFromEuler(Eigen::Vector3f(0.5f, -0.2f, -1.5f));
|
||||||
|
@ -498,6 +513,7 @@ int test_eigen(int argc, char *argv[]) {
|
||||||
rc = 1;
|
rc = 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/********************************************************************************************************/
|
/********************************************************************************************************/
|
||||||
/******************************************** TEST 4 ****************************************************/
|
/******************************************** TEST 4 ****************************************************/
|
||||||
q = quatFromEuler(Eigen::Vector3f(-M_PI_F / 3.0f, -M_PI_2_F, M_PI_2_F));
|
q = quatFromEuler(Eigen::Vector3f(-M_PI_F / 3.0f, -M_PI_2_F, M_PI_2_F));
|
||||||
|
@ -513,6 +529,7 @@ int test_eigen(int argc, char *argv[]) {
|
||||||
rc = 1;
|
rc = 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/********************************************************************************************************/
|
/********************************************************************************************************/
|
||||||
/******************************************** TEST 5 ****************************************************/
|
/******************************************** TEST 5 ****************************************************/
|
||||||
printf("%llu: Rotation method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
|
printf("%llu: Rotation method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
|
||||||
|
|
|
@ -176,6 +176,7 @@ int test_file2(int argc, char *argv[])
|
||||||
|
|
||||||
int myoptind = 1;
|
int myoptind = 1;
|
||||||
const char *myoptarg = NULL;
|
const char *myoptarg = NULL;
|
||||||
|
|
||||||
while ((opt = px4_getopt(argc, argv, "c:s:FLh", &myoptind, &myoptarg)) != EOF) {
|
while ((opt = px4_getopt(argc, argv, "c:s:FLh", &myoptind, &myoptarg)) != EOF) {
|
||||||
switch (opt) {
|
switch (opt) {
|
||||||
case 'F':
|
case 'F':
|
||||||
|
|
|
@ -216,6 +216,7 @@ int test_mixer(int argc, char *argv[])
|
||||||
warnx("active servo < min");
|
warnx("active servo < min");
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
|
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
|
||||||
warnx("throttle output != 0 (this check assumed the IO pass mixer!)");
|
warnx("throttle output != 0 (this check assumed the IO pass mixer!)");
|
||||||
|
@ -287,7 +288,8 @@ int test_mixer(int argc, char *argv[])
|
||||||
/* mix */
|
/* mix */
|
||||||
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
|
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
|
||||||
|
|
||||||
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
|
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
|
||||||
|
r_page_servo_control_max, outputs,
|
||||||
r_page_servos, &pwm_limit);
|
r_page_servos, &pwm_limit);
|
||||||
|
|
||||||
PX4_INFO("mixed %d outputs (max %d)", mixed, output_max);
|
PX4_INFO("mixed %d outputs (max %d)", mixed, output_max);
|
||||||
|
@ -314,7 +316,8 @@ int test_mixer(int argc, char *argv[])
|
||||||
/* mix */
|
/* mix */
|
||||||
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
|
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
|
||||||
|
|
||||||
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
|
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
|
||||||
|
r_page_servo_control_max, outputs,
|
||||||
r_page_servos, &pwm_limit);
|
r_page_servos, &pwm_limit);
|
||||||
|
|
||||||
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
|
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
|
||||||
|
@ -351,7 +354,8 @@ int test_mixer(int argc, char *argv[])
|
||||||
/* mix */
|
/* mix */
|
||||||
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
|
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
|
||||||
|
|
||||||
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
|
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
|
||||||
|
r_page_servo_control_max, outputs,
|
||||||
r_page_servos, &pwm_limit);
|
r_page_servos, &pwm_limit);
|
||||||
|
|
||||||
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
|
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
|
||||||
|
|
Loading…
Reference in New Issue