dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

format src/systemcmds/tests

This commit is contained in:
Daniel Agar 2015-09-05 12:21:12 -04:00
parent bed3fdf952
commit 2fdbdd15ec
8 changed files with 53 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/systemcmds/tests/test_bson.c
View File

@ -39,7 +39,7 @@
#define __STDC_FORMAT_MACROS
#include <inttypes.h>
#include <px4_defines.h>
#include <stdio.h>
#include <string.h>
@ -111,8 +111,8 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
if (node->b != sample_bool) {
PX4_ERR("FAIL: decoder: bool1 value %s, expected %s",
(node->b ? "true" : "false"),
(sample_bool ? "true" : "false"));
(node->b ? "true" : "false"),
(sample_bool ? "true" : "false"));
return 1;
}

2
src/systemcmds/tests/test_conv.cpp
View File

@ -67,7 +67,7 @@ int test_conv(int argc, char *argv[])
if (fabsf(f - fres) > 0.0001f) {
PX4_ERR("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)),
(double)fres);
(double)fres);
return 1;
}
}

53
src/systemcmds/tests/test_eigen.cpp
View File

@ -89,9 +89,9 @@ uint8_t err_num;
#define EXPECT_QUATERNION(q_exp, q_act, epsilon) \
(fabsf(q_exp.x() - q_act.x()) <= epsilon && \
fabsf(q_exp.y() - q_act.y()) <= epsilon && \
fabsf(q_exp.z() - q_act.z()) <= epsilon && \
fabsf(q_exp.w() - q_act.w()) <= epsilon)
fabsf(q_exp.y() - q_act.y()) <= epsilon && \
fabsf(q_exp.z() - q_act.z()) <= epsilon && \
fabsf(q_exp.w() - q_act.w()) <= epsilon)
#define EXPECT_NEAR(expected, actual, epsilon) \
(fabsf(expected - actual) <= epsilon)
@ -130,12 +130,13 @@ math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q);
* Construct new Eigen::Quaternion from euler angles
* Right order is YPR.
**/
Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy){
Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy)
{
return Eigen::Quaternionf(
Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(rpy.x(), Eigen::Vector3f::UnitX())
);
Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(rpy.x(), Eigen::Vector3f::UnitX())
);
}
/**
@ -143,7 +144,8 @@ Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy){
* Construct new Eigen::Vector3f of euler angles from quaternion
* 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();
}
@ -151,7 +153,8 @@ Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q){
* @brief
* 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();
}
@ -159,7 +162,8 @@ Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy){
* @brief
* 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]);
}
@ -167,7 +171,8 @@ Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q){
* @brief
* 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());
}
@ -175,7 +180,8 @@ math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q){
* @brief
* Testing main routine
**/
int test_eigen(int argc, char *argv[]) {
int test_eigen(int argc, char *argv[])
{
int rc = 0;
warnx("Testing Eigen math...");
@ -285,7 +291,7 @@ int test_eigen(int argc, char *argv[]) {
const Eigen::Matrix<float, 2, 3> m1_orig =
(Eigen::Matrix<float, 2, 3>() << 1, 2, 3,
4, 5, 6).finished();
4, 5, 6).finished();
Eigen::Matrix<float, 2, 3> m1(m1_orig);
@ -362,6 +368,7 @@ int test_eigen(int argc, char *argv[]) {
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 2 ****************************************************/
q_true = {1.0f, 0.0f, 0.0f, 0.0f};
@ -376,6 +383,7 @@ int test_eigen(int argc, char *argv[]) {
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 3 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(0.3f, 0.2f, 0.1f));
@ -388,6 +396,7 @@ int test_eigen(int argc, char *argv[]) {
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 4 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(-1.5f, -0.2f, 0.5f));
@ -400,6 +409,7 @@ int test_eigen(int argc, char *argv[]) {
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 5 ****************************************************/
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++) {
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("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;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 6 ****************************************************/
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 j = 0; j < 3; j++) {
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("Expected: \t%8.5f\n", R_orig(i, j));
++err_num;
@ -457,7 +470,7 @@ int test_eigen(int argc, char *argv[]) {
/******************************************** TEST 1 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.0f, 0.0f, M_PI_2_F));
vector_q = q._transformVector(vector);
Eigen::Vector3f vector_true = {-1.00f, 1.00f, 1.00f};
Eigen::Vector3f vector_true = { -1.00f, 1.00f, 1.00f};
for (size_t i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_true(i), vector_q(i), FLT_EPSILON)) {
@ -468,6 +481,7 @@ int test_eigen(int argc, char *argv[]) {
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 2 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.1f, 0.2f, 0.3f));
@ -483,6 +497,7 @@ int test_eigen(int argc, char *argv[]) {
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 3 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.5f, -0.2f, -1.5f));
@ -498,11 +513,12 @@ int test_eigen(int argc, char *argv[]) {
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 4 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(-M_PI_F / 3.0f, -M_PI_2_F, M_PI_2_F));
vector_q = q._transformVector(vector);
vector_true = {-1.366030f, 0.366025f, 1.000000f};
vector_true = { -1.366030f, 0.366025f, 1.000000f};
for (size_t i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
@ -513,6 +529,7 @@ int test_eigen(int argc, char *argv[]) {
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 5 ****************************************************/
printf("%llu: Rotation method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());

1
src/systemcmds/tests/test_file2.c
View File

@ -176,6 +176,7 @@ int test_file2(int argc, char *argv[])
int myoptind = 1;
const char *myoptarg = NULL;
while ((opt = px4_getopt(argc, argv, "c:s:FLh", &myoptind, &myoptarg)) != EOF) {
switch (opt) {
case 'F':

2
src/systemcmds/tests/test_hott_telemetry.c
View File

@ -195,7 +195,7 @@ int test_hott_telemetry(int argc, char *argv[])
} else {
PX4_WARN("WARN: Received %d out of %d byte pairs of which %d were valid from the HoTT receiver device.", received_count,
max_polls, valid_count);
max_polls, valid_count);
}
} else {

16
src/systemcmds/tests/test_mixer.cpp
View File

@ -204,7 +204,7 @@ int test_mixer(int argc, char *argv[])
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, r_page_servos, &pwm_limit);
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
@ -216,6 +216,7 @@ int test_mixer(int argc, char *argv[])
warnx("active servo < min");
return 1;
}
} else {
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
warnx("throttle output != 0 (this check assumed the IO pass mixer!)");
@ -244,7 +245,7 @@ int test_mixer(int argc, char *argv[])
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, r_page_servos, &pwm_limit);
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
@ -287,7 +288,8 @@ int test_mixer(int argc, char *argv[])
/* mix */
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);
PX4_INFO("mixed %d outputs (max %d)", mixed, output_max);
@ -314,7 +316,8 @@ int test_mixer(int argc, char *argv[])
/* mix */
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);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
@ -351,7 +354,8 @@ int test_mixer(int argc, char *argv[])
/* mix */
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);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
@ -435,7 +439,7 @@ mixer_callback(uintptr_t handle,
control = actuator_controls[control_index];
if (should_prearm && control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
control_index == actuator_controls_s::INDEX_THROTTLE) {
control = NAN_VALUE;
}

2
src/systemcmds/tests/test_mount.c
View File

@ -132,7 +132,7 @@ test_mount(int argc, char *argv[])
}
PX4_INFO("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort,
fsync_tries, abort_tries, buf);
fsync_tries, abort_tries, buf);
int it_left_fsync_prev = it_left_fsync;

2
src/systemcmds/tests/test_uart_baudchange.c
View File

@ -165,5 +165,5 @@ int test_uart_baudchange(int argc, char *argv[])
cleanup:
close(uart2);
return ret;
}