mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-06 16:49:51 +08:00
Tests: Fix code style on system tests
This commit is contained in:
@@ -58,18 +58,29 @@ static const uint8_t sample_data[256] = {0};
|
||||
static int
|
||||
encode(bson_encoder_t encoder)
|
||||
{
|
||||
if (bson_encoder_append_bool(encoder, "bool1", sample_bool) != 0)
|
||||
if (bson_encoder_append_bool(encoder, "bool1", sample_bool) != 0) {
|
||||
warnx("FAIL: encoder: append bool failed");
|
||||
if (bson_encoder_append_int(encoder, "int1", sample_small_int) != 0)
|
||||
}
|
||||
|
||||
if (bson_encoder_append_int(encoder, "int1", sample_small_int) != 0) {
|
||||
warnx("FAIL: encoder: append int failed");
|
||||
if (bson_encoder_append_int(encoder, "int2", sample_big_int) != 0)
|
||||
}
|
||||
|
||||
if (bson_encoder_append_int(encoder, "int2", sample_big_int) != 0) {
|
||||
warnx("FAIL: encoder: append int failed");
|
||||
if (bson_encoder_append_double(encoder, "double1", sample_double) != 0)
|
||||
}
|
||||
|
||||
if (bson_encoder_append_double(encoder, "double1", sample_double) != 0) {
|
||||
warnx("FAIL: encoder: append double failed");
|
||||
if (bson_encoder_append_string(encoder, "string1", sample_string) != 0)
|
||||
}
|
||||
|
||||
if (bson_encoder_append_string(encoder, "string1", sample_string) != 0) {
|
||||
warnx("FAIL: encoder: append string failed");
|
||||
if (bson_encoder_append_binary(encoder, "data1", BSON_BIN_BINARY, sizeof(sample_data), sample_data) != 0)
|
||||
}
|
||||
|
||||
if (bson_encoder_append_binary(encoder, "data1", BSON_BIN_BINARY, sizeof(sample_data), sample_data) != 0) {
|
||||
warnx("FAIL: encoder: append data failed");
|
||||
}
|
||||
|
||||
bson_encoder_fini(encoder);
|
||||
|
||||
@@ -86,51 +97,63 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
|
||||
warnx("FAIL: decoder: bool1 type %d, expected %d", node->type, BSON_BOOL);
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (node->b != sample_bool) {
|
||||
warnx("FAIL: decoder: bool1 value %s, expected %s",
|
||||
(node->b ? "true" : "false"),
|
||||
(sample_bool ? "true" : "false"));
|
||||
warnx("FAIL: decoder: bool1 value %s, expected %s",
|
||||
(node->b ? "true" : "false"),
|
||||
(sample_bool ? "true" : "false"));
|
||||
return 1;
|
||||
}
|
||||
|
||||
warnx("PASS: decoder: bool1");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!strcmp(node->name, "int1")) {
|
||||
if (node->type != BSON_INT32) {
|
||||
warnx("FAIL: decoder: int1 type %d, expected %d", node->type, BSON_INT32);
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (node->i != sample_small_int) {
|
||||
warnx("FAIL: decoder: int1 value %lld, expected %d", node->i, sample_small_int);
|
||||
return 1;
|
||||
}
|
||||
|
||||
warnx("PASS: decoder: int1");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!strcmp(node->name, "int2")) {
|
||||
if (node->type != BSON_INT64) {
|
||||
warnx("FAIL: decoder: int2 type %d, expected %d", node->type, BSON_INT64);
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (node->i != sample_big_int) {
|
||||
warnx("FAIL: decoder: int2 value %lld, expected %lld", node->i, sample_big_int);
|
||||
return 1;
|
||||
}
|
||||
|
||||
warnx("PASS: decoder: int2");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!strcmp(node->name, "double1")) {
|
||||
if (node->type != BSON_DOUBLE) {
|
||||
warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE);
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (fabs(node->d - sample_double) > 1e-12) {
|
||||
warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double);
|
||||
return 1;
|
||||
}
|
||||
|
||||
warnx("PASS: decoder: double1");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!strcmp(node->name, "string1")) {
|
||||
if (node->type != BSON_STRING) {
|
||||
warnx("FAIL: decoder: string1 type %d, expected %d", node->type, BSON_STRING);
|
||||
@@ -150,21 +173,26 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
|
||||
warnx("FAIL: decoder: string1 copy failed");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (bson_decoder_data_pending(decoder) != 0) {
|
||||
warnx("FAIL: decoder: string1 copy did not exhaust all data");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (sbuf[len - 1] != '\0') {
|
||||
warnx("FAIL: decoder: string1 not 0-terminated");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (strcmp(sbuf, sample_string)) {
|
||||
warnx("FAIL: decoder: string1 value '%s', expected '%s'", sbuf, sample_string);
|
||||
return 1;
|
||||
}
|
||||
|
||||
warnx("PASS: decoder: string1");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!strcmp(node->name, "data1")) {
|
||||
if (node->type != BSON_BINDATA) {
|
||||
warnx("FAIL: decoder: data1 type %d, expected %d", node->type, BSON_BINDATA);
|
||||
@@ -177,7 +205,7 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
|
||||
warnx("FAIL: decoder: data1 length %d, expected %d", len, sizeof(sample_data));
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
if (node->subtype != BSON_BIN_BINARY) {
|
||||
warnx("FAIL: decoder: data1 subtype %d, expected %d", node->subtype, BSON_BIN_BINARY);
|
||||
return 1;
|
||||
@@ -189,20 +217,25 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
|
||||
warnx("FAIL: decoder: data1 copy failed");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (bson_decoder_data_pending(decoder) != 0) {
|
||||
warnx("FAIL: decoder: data1 copy did not exhaust all data");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (memcmp(sample_data, dbuf, len)) {
|
||||
warnx("FAIL: decoder: data1 compare fail");
|
||||
return 1;
|
||||
}
|
||||
|
||||
warnx("PASS: decoder: data1");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (node->type != BSON_EOO)
|
||||
if (node->type != BSON_EOO) {
|
||||
warnx("FAIL: decoder: unexpected node name '%s'", node->name);
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
@@ -225,19 +258,28 @@ test_bson(int argc, char *argv[])
|
||||
int len;
|
||||
|
||||
/* encode data to a memory buffer */
|
||||
if (bson_encoder_init_buf(&encoder, NULL, 0))
|
||||
if (bson_encoder_init_buf(&encoder, NULL, 0)) {
|
||||
errx(1, "FAIL: bson_encoder_init_buf");
|
||||
}
|
||||
|
||||
encode(&encoder);
|
||||
len = bson_encoder_buf_size(&encoder);
|
||||
if (len <= 0)
|
||||
|
||||
if (len <= 0) {
|
||||
errx(1, "FAIL: bson_encoder_buf_len");
|
||||
}
|
||||
|
||||
buf = bson_encoder_buf_data(&encoder);
|
||||
if (buf == NULL)
|
||||
|
||||
if (buf == NULL) {
|
||||
errx(1, "FAIL: bson_encoder_buf_data");
|
||||
}
|
||||
|
||||
/* now test-decode it */
|
||||
if (bson_decoder_init_buf(&decoder, buf, len, decode_callback, NULL))
|
||||
if (bson_decoder_init_buf(&decoder, buf, len, decode_callback, NULL)) {
|
||||
errx(1, "FAIL: bson_decoder_init_buf");
|
||||
}
|
||||
|
||||
decode(&decoder);
|
||||
free(buf);
|
||||
|
||||
|
||||
@@ -61,11 +61,13 @@ int test_conv(int argc, char *argv[])
|
||||
{
|
||||
warnx("Testing system conversions");
|
||||
|
||||
for (int i = -10000; i <= 10000; i+=1) {
|
||||
float f = i/10000.0f;
|
||||
for (int i = -10000; i <= 10000; i += 1) {
|
||||
float f = i / 10000.0f;
|
||||
float fres = REG_TO_FLOAT(FLOAT_TO_REG(f));
|
||||
|
||||
if (fabsf(f - fres) > 0.0001f) {
|
||||
warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres);
|
||||
warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)),
|
||||
(double)fres);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -70,19 +70,23 @@ task_main(int argc, char *argv[])
|
||||
warnx("Starting dataman test task %s", argv[1]);
|
||||
/* try to read an invalid item */
|
||||
int my_id = atoi(argv[1]);
|
||||
|
||||
/* try to read an invalid item */
|
||||
if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) {
|
||||
warnx("%d read an invalid item failed", my_id);
|
||||
goto fail;
|
||||
}
|
||||
|
||||
/* try to read an invalid index */
|
||||
if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) {
|
||||
warnx("%d read an invalid index failed", my_id);
|
||||
goto fail;
|
||||
}
|
||||
|
||||
srand(hrt_absolute_time() ^ my_id);
|
||||
unsigned hit = 0, miss = 0;
|
||||
wstart = hrt_absolute_time();
|
||||
|
||||
for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
|
||||
memset(buffer, my_id, sizeof(buffer));
|
||||
buffer[1] = i;
|
||||
@@ -91,44 +95,53 @@ task_main(int argc, char *argv[])
|
||||
|
||||
int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len);
|
||||
warnx("ret: %d", ret);
|
||||
|
||||
if (ret != len) {
|
||||
warnx("%d write failed, index %d, length %d", my_id, hash, len);
|
||||
goto fail;
|
||||
}
|
||||
|
||||
usleep(rand() & ((64 * 1024) - 1));
|
||||
}
|
||||
|
||||
rstart = hrt_absolute_time();
|
||||
wend = rstart;
|
||||
|
||||
for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
|
||||
unsigned hash = i ^ my_id;
|
||||
unsigned len2, len = (hash & 63) + 2;
|
||||
|
||||
if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) {
|
||||
warnx("%d read failed length test, index %d", my_id, hash);
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (buffer[0] == my_id) {
|
||||
hit++;
|
||||
|
||||
if (len2 != len) {
|
||||
warnx("%d read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2);
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (buffer[1] != i) {
|
||||
warnx("%d data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]);
|
||||
goto fail;
|
||||
}
|
||||
}
|
||||
else
|
||||
|
||||
} else {
|
||||
miss++;
|
||||
}
|
||||
}
|
||||
|
||||
rend = hrt_absolute_time();
|
||||
warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.",
|
||||
my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000);
|
||||
my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000);
|
||||
sem_post(sems + my_id);
|
||||
return 0;
|
||||
fail:
|
||||
warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x",
|
||||
my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
|
||||
my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
|
||||
sem_post(sems + my_id);
|
||||
return -1;
|
||||
}
|
||||
@@ -138,11 +151,13 @@ int test_dataman(int argc, char *argv[])
|
||||
int i, num_tasks = 4;
|
||||
char buffer[DM_MAX_DATA_SIZE];
|
||||
|
||||
if (argc > 1)
|
||||
if (argc > 1) {
|
||||
num_tasks = atoi(argv[1]);
|
||||
|
||||
}
|
||||
|
||||
sems = (sem_t *)malloc(num_tasks * sizeof(sem_t));
|
||||
warnx("Running %d tasks", num_tasks);
|
||||
|
||||
for (i = 0; i < num_tasks; i++) {
|
||||
int task;
|
||||
char a[16];
|
||||
@@ -151,32 +166,41 @@ int test_dataman(int argc, char *argv[])
|
||||
av[0] = a;
|
||||
av[1] = 0;
|
||||
sem_init(sems + i, 1, 0);
|
||||
|
||||
/* start the task */
|
||||
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) {
|
||||
warn("task start failed");
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < num_tasks; i++) {
|
||||
sem_wait(sems + i);
|
||||
sem_destroy(sems + i);
|
||||
}
|
||||
|
||||
free(sems);
|
||||
dm_restart(DM_INIT_REASON_IN_FLIGHT);
|
||||
|
||||
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
|
||||
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0)
|
||||
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (i >= NUM_MISSIONS_SUPPORTED) {
|
||||
warnx("Restart in-flight failed");
|
||||
return -1;
|
||||
|
||||
}
|
||||
|
||||
dm_restart(DM_INIT_REASON_POWER_ON);
|
||||
|
||||
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
|
||||
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
|
||||
warnx("Restart power-on failed");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -52,27 +52,27 @@
|
||||
|
||||
namespace Eigen
|
||||
{
|
||||
typedef Matrix<float, 10, 1> Vector10f;
|
||||
typedef Matrix<float, 10, 1> Vector10f;
|
||||
}
|
||||
|
||||
static constexpr size_t OPERATOR_ITERATIONS = 60000;
|
||||
|
||||
#define TEST_OP(_title, _op) \
|
||||
{ \
|
||||
const hrt_abstime t0 = hrt_absolute_time(); \
|
||||
for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \
|
||||
_op; \
|
||||
} \
|
||||
printf(_title ": %.6fus", static_cast<double>(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \
|
||||
}
|
||||
{ \
|
||||
const hrt_abstime t0 = hrt_absolute_time(); \
|
||||
for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \
|
||||
_op; \
|
||||
} \
|
||||
printf(_title ": %.6fus", static_cast<double>(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \
|
||||
}
|
||||
|
||||
#define VERIFY_OP(_title, _op, __OP_TEST__) \
|
||||
{ \
|
||||
_op; \
|
||||
if(!(__OP_TEST__)) { \
|
||||
printf(_title " Failed! ("#__OP_TEST__")"); \
|
||||
} \
|
||||
}
|
||||
{ \
|
||||
_op; \
|
||||
if(!(__OP_TEST__)) { \
|
||||
printf(_title " Failed! ("#__OP_TEST__")"); \
|
||||
} \
|
||||
}
|
||||
|
||||
#define TEST_OP_VERIFY(_title, _op, __OP_TEST__) \
|
||||
VERIFY_OP(_title, _op, __OP_TEST__) \
|
||||
@@ -83,18 +83,20 @@ static constexpr size_t OPERATOR_ITERATIONS = 60000;
|
||||
* Prints an Eigen::Matrix to stdout
|
||||
**/
|
||||
template<typename T>
|
||||
void printEigen(const Eigen::MatrixBase<T>& b)
|
||||
void printEigen(const Eigen::MatrixBase<T> &b)
|
||||
{
|
||||
for(int i = 0; i < b.rows(); ++i) {
|
||||
for (int i = 0; i < b.rows(); ++i) {
|
||||
printf("(");
|
||||
for(int j = 0; j < b.cols(); ++i) {
|
||||
if(j > 0) {
|
||||
|
||||
for (int j = 0; j < b.cols(); ++i) {
|
||||
if (j > 0) {
|
||||
printf(",");
|
||||
}
|
||||
|
||||
printf("%.3f", static_cast<double>(b(i, j)));
|
||||
}
|
||||
printf(")%s\n", i+1 < b.rows() ? "," : "");
|
||||
|
||||
printf(")%s\n", i + 1 < b.rows() ? "," : "");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -108,7 +110,7 @@ Eigen::Quaternion<T> quatFromEuler(const T roll, const T pitch, const T yaw)
|
||||
Eigen::AngleAxis<T> rollAngle(roll, Eigen::Matrix<T, 3, 1>::UnitZ());
|
||||
Eigen::AngleAxis<T> yawAngle(yaw, Eigen::Matrix<T, 3, 1>::UnitY());
|
||||
Eigen::AngleAxis<T> pitchAngle(pitch, Eigen::Matrix<T, 3, 1>::UnitX());
|
||||
return rollAngle * yawAngle * pitchAngle;
|
||||
return rollAngle * yawAngle * pitchAngle;
|
||||
}
|
||||
|
||||
|
||||
@@ -127,9 +129,9 @@ int test_eigen(int argc, char *argv[])
|
||||
TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]);
|
||||
TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f);
|
||||
TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1));
|
||||
TEST_OP_VERIFY("Vector2f + Vector2f", v + v1, v.isApprox(v1+v1));
|
||||
TEST_OP_VERIFY("Vector2f + Vector2f", v + v1, v.isApprox(v1 + v1));
|
||||
TEST_OP_VERIFY("Vector2f - Vector2f", v - v1, v.isApprox(v1));
|
||||
TEST_OP_VERIFY("Vector2f += Vector2f", v += v1, v.isApprox(v1+v1));
|
||||
TEST_OP_VERIFY("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1));
|
||||
TEST_OP_VERIFY("Vector2f -= Vector2f", v -= v1, v.isApprox(v1));
|
||||
TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON);
|
||||
//TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array?
|
||||
@@ -185,7 +187,7 @@ int test_eigen(int argc, char *argv[])
|
||||
TEST_OP("Vector<4> dot Vector<4>", v.dot(v1));
|
||||
}
|
||||
|
||||
{
|
||||
{
|
||||
Eigen::Vector10f v1;
|
||||
v1.Zero();
|
||||
float data[10];
|
||||
@@ -222,18 +224,18 @@ int test_eigen(int argc, char *argv[])
|
||||
// test nonsymmetric +, -, +=, -=
|
||||
|
||||
const Eigen::Matrix<float, 2, 3> m1_orig =
|
||||
(Eigen::Matrix<float, 2, 3>() << 1, 3, 3,
|
||||
4, 6, 6).finished();
|
||||
(Eigen::Matrix<float, 2, 3>() << 1, 3, 3,
|
||||
4, 6, 6).finished();
|
||||
|
||||
Eigen::Matrix<float, 2, 3> m1(m1_orig);
|
||||
|
||||
Eigen::Matrix<float, 2, 3> m2;
|
||||
m2 << 2, 4, 6,
|
||||
8, 10, 12;
|
||||
8, 10, 12;
|
||||
|
||||
Eigen::Matrix<float, 2, 3> m3;
|
||||
m3 << 3, 6, 9,
|
||||
12, 15, 18;
|
||||
12, 15, 18;
|
||||
|
||||
if (m1 + m2 != m3) {
|
||||
warnx("Matrix<2, 3> + Matrix<2, 3> failed!");
|
||||
@@ -349,7 +351,7 @@ int test_eigen(int argc, char *argv[])
|
||||
|
||||
{
|
||||
// test quaternion method "rotate" (rotate vector by quaternion)
|
||||
Eigen::Vector3f vector = {1.0f,1.0f,1.0f};
|
||||
Eigen::Vector3f vector = {1.0f, 1.0f, 1.0f};
|
||||
Eigen::Vector3f vector_q;
|
||||
Eigen::Vector3f vector_r;
|
||||
Eigen::Quaternionf q;
|
||||
@@ -363,11 +365,12 @@ int test_eigen(int argc, char *argv[])
|
||||
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
|
||||
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
|
||||
R.eulerAngles(roll, pitch, yaw);
|
||||
q = quatFromEuler(roll,pitch,yaw);
|
||||
vector_r = R*vector;
|
||||
q = quatFromEuler(roll, pitch, yaw);
|
||||
vector_r = R * vector;
|
||||
vector_q = q._transformVector(vector);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if(fabsf(vector_r(i) - vector_q(i)) > tol) {
|
||||
if (fabsf(vector_r(i) - vector_q(i)) > tol) {
|
||||
warnx("Quaternion method 'rotate' outside tolerance");
|
||||
rc = 1;
|
||||
}
|
||||
@@ -378,41 +381,45 @@ int test_eigen(int argc, char *argv[])
|
||||
|
||||
// test some values calculated with matlab
|
||||
tol = 0.0001f;
|
||||
q = quatFromEuler(M_PI_2_F,0.0f,0.0f);
|
||||
q = quatFromEuler(M_PI_2_F, 0.0f, 0.0f);
|
||||
vector_q = q._transformVector(vector);
|
||||
Eigen::Vector3f vector_true = {1.00f,-1.00f,1.00f};
|
||||
for(size_t i = 0;i<3;i++) {
|
||||
if(fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
Eigen::Vector3f vector_true = {1.00f, -1.00f, 1.00f};
|
||||
|
||||
for (size_t i = 0; i < 3; i++) {
|
||||
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
warnx("Quaternion method 'rotate' outside tolerance");
|
||||
rc = 1;
|
||||
}
|
||||
}
|
||||
|
||||
q = quatFromEuler(0.3f,0.2f,0.1f);
|
||||
q = quatFromEuler(0.3f, 0.2f, 0.1f);
|
||||
vector_q = q._transformVector(vector);
|
||||
vector_true = {1.1566,0.7792,1.0273};
|
||||
for(size_t i = 0;i<3;i++) {
|
||||
if(fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
vector_true = {1.1566, 0.7792, 1.0273};
|
||||
|
||||
for (size_t i = 0; i < 3; i++) {
|
||||
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
warnx("Quaternion method 'rotate' outside tolerance");
|
||||
rc = 1;
|
||||
}
|
||||
}
|
||||
|
||||
q = quatFromEuler(-1.5f,-0.2f,0.5f);
|
||||
q = quatFromEuler(-1.5f, -0.2f, 0.5f);
|
||||
vector_q = q._transformVector(vector);
|
||||
vector_true = {0.5095,1.4956,-0.7096};
|
||||
for(size_t i = 0;i<3;i++) {
|
||||
if(fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
vector_true = {0.5095, 1.4956, -0.7096};
|
||||
|
||||
for (size_t i = 0; i < 3; i++) {
|
||||
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
warnx("Quaternion method 'rotate' outside tolerance");
|
||||
rc = 1;
|
||||
}
|
||||
}
|
||||
|
||||
q = quatFromEuler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3.0f);
|
||||
q = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
|
||||
vector_q = q._transformVector(vector);
|
||||
vector_true = {-1.3660,0.3660,1.0000};
|
||||
for(size_t i = 0;i<3;i++) {
|
||||
if(fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
vector_true = { -1.3660, 0.3660, 1.0000};
|
||||
|
||||
for (size_t i = 0; i < 3; i++) {
|
||||
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
warnx("Quaternion method 'rotate' outside tolerance");
|
||||
rc = 1;
|
||||
}
|
||||
|
||||
@@ -56,7 +56,8 @@
|
||||
|
||||
static int check_user_abort(int fd);
|
||||
|
||||
int check_user_abort(int fd) {
|
||||
int check_user_abort(int fd)
|
||||
{
|
||||
/* check if user wants to abort */
|
||||
char c;
|
||||
|
||||
@@ -74,13 +75,12 @@ int check_user_abort(int fd) {
|
||||
case 0x03: // ctrl-c
|
||||
case 0x1b: // esc
|
||||
case 'c':
|
||||
case 'q':
|
||||
{
|
||||
warnx("Test aborted.");
|
||||
fsync(fd);
|
||||
close(fd);
|
||||
return OK;
|
||||
/* not reached */
|
||||
case 'q': {
|
||||
warnx("Test aborted.");
|
||||
fsync(fd);
|
||||
close(fd);
|
||||
return OK;
|
||||
/* not reached */
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -96,6 +96,7 @@ test_file(int argc, char *argv[])
|
||||
|
||||
/* check if microSD card is mounted */
|
||||
struct stat buffer;
|
||||
|
||||
if (stat("/fs/microsd/", &buffer)) {
|
||||
warnx("no microSD card mounted, aborting file test");
|
||||
return 1;
|
||||
@@ -118,7 +119,7 @@ test_file(int argc, char *argv[])
|
||||
/* fill write buffer with known values */
|
||||
for (size_t i = 0; i < sizeof(write_buf); i++) {
|
||||
/* this will wrap, but we just need a known value with spacing */
|
||||
write_buf[i] = i+11;
|
||||
write_buf[i] = i + 11;
|
||||
}
|
||||
|
||||
uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
|
||||
@@ -129,24 +130,28 @@ test_file(int argc, char *argv[])
|
||||
warnx("testing unaligned writes - please wait..");
|
||||
|
||||
start = hrt_absolute_time();
|
||||
|
||||
for (unsigned i = 0; i < iterations; i++) {
|
||||
int wret = write(fd, write_buf + a, chunk_sizes[c]);
|
||||
|
||||
if (wret != chunk_sizes[c]) {
|
||||
warn("WRITE ERROR!");
|
||||
|
||||
if ((0x3 & (uintptr_t)(write_buf + a)))
|
||||
if ((0x3 & (uintptr_t)(write_buf + a))) {
|
||||
warnx("memory is unaligned, align shift: %d", a);
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
fsync(fd);
|
||||
|
||||
if (!check_user_abort(fd))
|
||||
if (!check_user_abort(fd)) {
|
||||
return OK;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
end = hrt_absolute_time();
|
||||
|
||||
warnx("write took %llu us", (end - start));
|
||||
@@ -162,7 +167,7 @@ test_file(int argc, char *argv[])
|
||||
warnx("READ ERROR!");
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
/* compare value */
|
||||
bool compare_ok = true;
|
||||
|
||||
@@ -179,8 +184,9 @@ test_file(int argc, char *argv[])
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!check_user_abort(fd))
|
||||
if (!check_user_abort(fd)) {
|
||||
return OK;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -202,8 +208,9 @@ test_file(int argc, char *argv[])
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!check_user_abort(fd))
|
||||
if (!check_user_abort(fd)) {
|
||||
return OK;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -224,7 +231,7 @@ test_file(int argc, char *argv[])
|
||||
warnx("READ ERROR!");
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
for (int j = 0; j < chunk_sizes[c]; j++) {
|
||||
if (read_buf[j] != write_buf[j]) {
|
||||
warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]);
|
||||
@@ -232,8 +239,9 @@ test_file(int argc, char *argv[])
|
||||
break;
|
||||
}
|
||||
|
||||
if (!check_user_abort(fd))
|
||||
if (!check_user_abort(fd)) {
|
||||
return OK;
|
||||
}
|
||||
}
|
||||
|
||||
if (!align_read_ok) {
|
||||
@@ -267,16 +275,19 @@ test_file(int argc, char *argv[])
|
||||
for (int j = 0; j < chunk_sizes[c]; j++) {
|
||||
|
||||
if ((read_buf + a)[j] != write_buf[j]) {
|
||||
warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]);
|
||||
warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a],
|
||||
(unsigned int)write_buf[j]);
|
||||
unalign_read_ok = false;
|
||||
unalign_read_err_count++;
|
||||
|
||||
if (unalign_read_err_count > 10)
|
||||
|
||||
if (unalign_read_err_count > 10) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!check_user_abort(fd))
|
||||
if (!check_user_abort(fd)) {
|
||||
return OK;
|
||||
}
|
||||
}
|
||||
|
||||
if (!unalign_read_ok) {
|
||||
@@ -300,6 +311,7 @@ test_file(int argc, char *argv[])
|
||||
DIR *d;
|
||||
struct dirent *dir;
|
||||
d = opendir("/fs/microsd");
|
||||
|
||||
if (d) {
|
||||
|
||||
while ((dir = readdir(d)) != NULL) {
|
||||
|
||||
@@ -69,42 +69,51 @@ static uint8_t get_value(uint32_t ofs)
|
||||
|
||||
static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t write_size, uint16_t flags)
|
||||
{
|
||||
printf("Testing on %s with write_chunk=%u write_size=%u\n",
|
||||
printf("Testing on %s with write_chunk=%u write_size=%u\n",
|
||||
filename, (unsigned)write_chunk, (unsigned)write_size);
|
||||
|
||||
|
||||
uint32_t ofs = 0;
|
||||
int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC);
|
||||
|
||||
if (fd == -1) {
|
||||
perror(filename);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
// create a file of size write_size, in write_chunk blocks
|
||||
uint8_t counter = 0;
|
||||
|
||||
while (ofs < write_size) {
|
||||
uint8_t buffer[write_chunk];
|
||||
for (uint16_t j=0; j<write_chunk; j++) {
|
||||
buffer[j] = get_value(ofs);
|
||||
ofs++;
|
||||
}
|
||||
if (write(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) {
|
||||
printf("write failed at offset %u\n", ofs);
|
||||
exit(1);
|
||||
}
|
||||
if (flags & FLAG_FSYNC) {
|
||||
fsync(fd);
|
||||
}
|
||||
if (counter % 100 == 0) {
|
||||
printf("write ofs=%u\r", ofs);
|
||||
}
|
||||
counter++;
|
||||
uint8_t buffer[write_chunk];
|
||||
|
||||
for (uint16_t j = 0; j < write_chunk; j++) {
|
||||
buffer[j] = get_value(ofs);
|
||||
ofs++;
|
||||
}
|
||||
|
||||
if (write(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) {
|
||||
printf("write failed at offset %u\n", ofs);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
if (flags & FLAG_FSYNC) {
|
||||
fsync(fd);
|
||||
}
|
||||
|
||||
if (counter % 100 == 0) {
|
||||
printf("write ofs=%u\r", ofs);
|
||||
}
|
||||
|
||||
counter++;
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
|
||||
printf("write ofs=%u\n", ofs);
|
||||
|
||||
|
||||
// read and check
|
||||
fd = open(filename, O_RDONLY);
|
||||
|
||||
if (fd == -1) {
|
||||
perror(filename);
|
||||
exit(1);
|
||||
@@ -112,31 +121,39 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t
|
||||
|
||||
counter = 0;
|
||||
ofs = 0;
|
||||
|
||||
while (ofs < write_size) {
|
||||
uint8_t buffer[write_chunk];
|
||||
|
||||
if (counter % 100 == 0) {
|
||||
printf("read ofs=%u\r", ofs);
|
||||
}
|
||||
|
||||
counter++;
|
||||
|
||||
if (read(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) {
|
||||
printf("read failed at offset %u\n", ofs);
|
||||
exit(1);
|
||||
}
|
||||
for (uint16_t j=0; j<write_chunk; j++) {
|
||||
|
||||
for (uint16_t j = 0; j < write_chunk; j++) {
|
||||
if (buffer[j] != get_value(ofs)) {
|
||||
printf("corruption at ofs=%u got %u\n", ofs, buffer[j]);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
ofs++;
|
||||
}
|
||||
|
||||
if (flags & FLAG_LSEEK) {
|
||||
lseek(fd, 0, SEEK_CUR);
|
||||
}
|
||||
}
|
||||
|
||||
printf("read ofs=%u\n", ofs);
|
||||
close(fd);
|
||||
unlink(filename);
|
||||
printf("All OK\n");
|
||||
printf("All OK\n");
|
||||
}
|
||||
|
||||
static void usage(void)
|
||||
@@ -155,22 +172,26 @@ int test_file2(int argc, char *argv[])
|
||||
uint16_t flags = 0;
|
||||
const char *filename = "/fs/microsd/testfile2.dat";
|
||||
uint32_t write_chunk = 64;
|
||||
uint32_t write_size = 5*1024;
|
||||
uint32_t write_size = 5 * 1024;
|
||||
|
||||
while ((opt = getopt(argc, argv, "c:s:FLh")) != EOF) {
|
||||
switch (opt) {
|
||||
case 'F':
|
||||
flags |= FLAG_FSYNC;
|
||||
break;
|
||||
|
||||
case 'L':
|
||||
flags |= FLAG_LSEEK;
|
||||
break;
|
||||
|
||||
case 's':
|
||||
write_size = strtoul(optarg, NULL, 0);
|
||||
break;
|
||||
|
||||
case 'c':
|
||||
write_chunk = strtoul(optarg, NULL, 0);
|
||||
break;
|
||||
|
||||
case 'h':
|
||||
default:
|
||||
usage();
|
||||
@@ -187,12 +208,13 @@ int test_file2(int argc, char *argv[])
|
||||
|
||||
/* check if microSD card is mounted */
|
||||
struct stat buffer;
|
||||
|
||||
if (stat("/fs/microsd/", &buffer)) {
|
||||
warnx("no microSD card mounted, aborting file test");
|
||||
return 1;
|
||||
}
|
||||
|
||||
test_corruption(filename, write_chunk, write_size, flags);
|
||||
return 0;
|
||||
test_corruption(filename, write_chunk, write_size, flags);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -158,20 +158,22 @@ int test_float(int argc, char *argv[])
|
||||
sprintf(sbuf, "%8.4f", (double)0.553415f);
|
||||
|
||||
if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
|
||||
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
|
||||
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
|
||||
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
|
||||
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
|
||||
printf("\t success: printf(\"%%8.4f\", 0.553415f) == %8.4f\n", (double)0.553415f);
|
||||
|
||||
} else {
|
||||
printf("\t FAIL: printf(\"%%8.4f\", 0.553415f) != \" 0.5534\", result: %s\n", sbuf);
|
||||
ret = -5;
|
||||
}
|
||||
|
||||
sprintf(sbuf, "%8.4f", (double)-0.553415f);
|
||||
sprintf(sbuf, "%8.4f", (double) - 0.553415f);
|
||||
|
||||
if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
|
||||
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
|
||||
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
|
||||
printf("\t success: printf(\"%%8.4f\", -0.553415f) == %8.4f\n", (double)-0.553415f);
|
||||
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
|
||||
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
|
||||
printf("\t success: printf(\"%%8.4f\", -0.553415f) == %8.4f\n", (double) - 0.553415f);
|
||||
|
||||
} else {
|
||||
printf("\t FAIL: printf(\"%%8.4f\", -0.553415f) != \" -0.5534\", result: %s\n", sbuf);
|
||||
ret = -6;
|
||||
@@ -250,9 +252,10 @@ int test_float(int argc, char *argv[])
|
||||
sprintf(sbuf, "%8.4f", 0.553415);
|
||||
|
||||
if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
|
||||
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
|
||||
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
|
||||
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
|
||||
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
|
||||
printf("\t success: printf(\"%%8.4f\", 0.553415) == %8.4f\n", 0.553415);
|
||||
|
||||
} else {
|
||||
printf("\t FAIL: printf(\"%%8.4f\", 0.553415) != \" 0.5534\", result: %s\n", sbuf);
|
||||
ret = -12;
|
||||
@@ -261,9 +264,10 @@ int test_float(int argc, char *argv[])
|
||||
sprintf(sbuf, "%8.4f", -0.553415);
|
||||
|
||||
if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
|
||||
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
|
||||
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
|
||||
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
|
||||
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
|
||||
printf("\t success: printf(\"%%8.4f\", -0.553415) == %8.4f\n", -0.553415);
|
||||
|
||||
} else {
|
||||
printf("\t FAIL: printf(\"%%8.4f\", -0.553415) != \" -0.5534\", result: %s\n", sbuf);
|
||||
ret = -13;
|
||||
|
||||
@@ -99,18 +99,21 @@ int test_ppm(int argc, char *argv[])
|
||||
|
||||
printf("channels: %u\n", ppm_decoded_channels);
|
||||
|
||||
for (i = 0; i < ppm_decoded_channels; i++)
|
||||
for (i = 0; i < ppm_decoded_channels; i++) {
|
||||
printf(" %u\n", ppm_buffer[i]);
|
||||
}
|
||||
|
||||
printf("edges\n");
|
||||
|
||||
for (i = 0; i < 32; i++)
|
||||
for (i = 0; i < 32; i++) {
|
||||
printf(" %u\n", ppm_edge_history[i]);
|
||||
}
|
||||
|
||||
printf("pulses\n");
|
||||
|
||||
for (i = 0; i < 32; i++)
|
||||
for (i = 0; i < 32; i++) {
|
||||
printf(" %u\n", ppm_pulse_history[i]);
|
||||
}
|
||||
|
||||
fflush(stdout);
|
||||
#else
|
||||
@@ -133,8 +136,9 @@ int test_tone(int argc, char *argv[])
|
||||
|
||||
tone = 1;
|
||||
|
||||
if (argc == 2)
|
||||
if (argc == 2) {
|
||||
tone = atoi(argv[1]);
|
||||
}
|
||||
|
||||
if (tone == 0) {
|
||||
result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE);
|
||||
@@ -167,8 +171,9 @@ int test_tone(int argc, char *argv[])
|
||||
|
||||
out:
|
||||
|
||||
if (fd >= 0)
|
||||
if (fd >= 0) {
|
||||
close(fd);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -284,11 +284,11 @@ int test_mathlib(int argc, char *argv[])
|
||||
|
||||
{
|
||||
// test quaternion method "rotate" (rotate vector by quaternion)
|
||||
Vector<3> vector = {1.0f,1.0f,1.0f};
|
||||
Vector<3> vector = {1.0f, 1.0f, 1.0f};
|
||||
Vector<3> vector_q;
|
||||
Vector<3> vector_r;
|
||||
Quaternion q;
|
||||
Matrix<3,3> R;
|
||||
Matrix<3, 3> R;
|
||||
float diff = 0.1f;
|
||||
float tol = 0.00001f;
|
||||
|
||||
@@ -298,11 +298,12 @@ int test_mathlib(int argc, char *argv[])
|
||||
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
|
||||
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
|
||||
R.from_euler(roll, pitch, yaw);
|
||||
q.from_euler(roll,pitch,yaw);
|
||||
vector_r = R*vector;
|
||||
q.from_euler(roll, pitch, yaw);
|
||||
vector_r = R * vector;
|
||||
vector_q = q.rotate(vector);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if(fabsf(vector_r(i) - vector_q(i)) > tol) {
|
||||
if (fabsf(vector_r(i) - vector_q(i)) > tol) {
|
||||
warnx("Quaternion method 'rotate' outside tolerance");
|
||||
rc = 1;
|
||||
}
|
||||
@@ -313,41 +314,45 @@ int test_mathlib(int argc, char *argv[])
|
||||
|
||||
// test some values calculated with matlab
|
||||
tol = 0.0001f;
|
||||
q.from_euler(M_PI_2_F,0.0f,0.0f);
|
||||
q.from_euler(M_PI_2_F, 0.0f, 0.0f);
|
||||
vector_q = q.rotate(vector);
|
||||
Vector<3> vector_true = {1.00f,-1.00f,1.00f};
|
||||
for(unsigned i = 0;i<3;i++) {
|
||||
if(fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
warnx("Quaternion method 'rotate' outside tolerance");
|
||||
rc = 1;
|
||||
}
|
||||
}
|
||||
|
||||
q.from_euler(0.3f,0.2f,0.1f);
|
||||
q.from_euler(0.3f, 0.2f, 0.1f);
|
||||
vector_q = q.rotate(vector);
|
||||
vector_true = {1.1566,0.7792,1.0273};
|
||||
for(unsigned i = 0;i<3;i++) {
|
||||
if(fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
vector_true = {1.1566, 0.7792, 1.0273};
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
warnx("Quaternion method 'rotate' outside tolerance");
|
||||
rc = 1;
|
||||
}
|
||||
}
|
||||
|
||||
q.from_euler(-1.5f,-0.2f,0.5f);
|
||||
q.from_euler(-1.5f, -0.2f, 0.5f);
|
||||
vector_q = q.rotate(vector);
|
||||
vector_true = {0.5095,1.4956,-0.7096};
|
||||
for(unsigned i = 0;i<3;i++) {
|
||||
if(fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
vector_true = {0.5095, 1.4956, -0.7096};
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
warnx("Quaternion method 'rotate' outside tolerance");
|
||||
rc = 1;
|
||||
}
|
||||
}
|
||||
|
||||
q.from_euler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3.0f);
|
||||
q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
|
||||
vector_q = q.rotate(vector);
|
||||
vector_true = {-1.3660,0.3660,1.0000};
|
||||
for(unsigned i = 0;i<3;i++) {
|
||||
if(fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
vector_true = { -1.3660, 0.3660, 1.0000};
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
|
||||
warnx("Quaternion method 'rotate' outside tolerance");
|
||||
rc = 1;
|
||||
}
|
||||
|
||||
@@ -83,8 +83,9 @@ int test_mixer(int argc, char *argv[])
|
||||
|
||||
const char *filename = "/etc/mixers/IO_pass.mix";
|
||||
|
||||
if (argc > 1)
|
||||
if (argc > 1) {
|
||||
filename = argv[1];
|
||||
}
|
||||
|
||||
warnx("loading: %s", filename);
|
||||
|
||||
@@ -108,8 +109,10 @@ int test_mixer(int argc, char *argv[])
|
||||
unsigned xx = loaded;
|
||||
mixer_group.load_from_buf(&buf[0], xx);
|
||||
warnx("complete buffer load: loaded %u mixers", mixer_group.count());
|
||||
if (mixer_group.count() != 8)
|
||||
|
||||
if (mixer_group.count() != 8) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
unsigned empty_load = 2;
|
||||
char empty_buf[2];
|
||||
@@ -118,8 +121,10 @@ int test_mixer(int argc, char *argv[])
|
||||
mixer_group.reset();
|
||||
mixer_group.load_from_buf(&empty_buf[0], empty_load);
|
||||
warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
|
||||
if (empty_load != 0)
|
||||
|
||||
if (empty_load != 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* FIRST mark the mixer as invalid */
|
||||
/* THEN actually delete it */
|
||||
@@ -155,8 +160,9 @@ int test_mixer(int argc, char *argv[])
|
||||
warnx("used %u", mixer_text_length - resid);
|
||||
|
||||
/* copy any leftover text to the base of the buffer for re-use */
|
||||
if (resid > 0)
|
||||
if (resid > 0) {
|
||||
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
|
||||
}
|
||||
|
||||
mixer_text_length = resid;
|
||||
}
|
||||
@@ -166,11 +172,12 @@ int test_mixer(int argc, char *argv[])
|
||||
|
||||
warnx("chunked load: loaded %u mixers", mixer_group.count());
|
||||
|
||||
if (mixer_group.count() != 8)
|
||||
if (mixer_group.count() != 8) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* execute the mixer */
|
||||
|
||||
|
||||
float outputs[output_max];
|
||||
unsigned mixed;
|
||||
const int jmax = 5;
|
||||
@@ -193,30 +200,31 @@ int test_mixer(int argc, char *argv[])
|
||||
unsigned sleepcount = 0;
|
||||
|
||||
while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) {
|
||||
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_group.mix(&outputs[0], output_max);
|
||||
|
||||
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
pwm_limit_calc(should_arm, mixed, 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);
|
||||
for (unsigned i = 0; i < mixed; i++)
|
||||
{
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
/* check mixed outputs to be zero during init phase */
|
||||
if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
|
||||
r_page_servos[i] != r_page_servo_disarmed[i]) {
|
||||
r_page_servos[i] != r_page_servo_disarmed[i]) {
|
||||
warnx("disarmed servo value mismatch");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (hrt_elapsed_time(&starttime) >= INIT_TIME_US &&
|
||||
r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
|
||||
r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
|
||||
warnx("ramp servo value mismatch");
|
||||
return 1;
|
||||
}
|
||||
|
||||
//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]);
|
||||
}
|
||||
|
||||
usleep(sleep_quantum_us);
|
||||
sleepcount++;
|
||||
|
||||
@@ -225,6 +233,7 @@ int test_mixer(int argc, char *argv[])
|
||||
fflush(stdout);
|
||||
}
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
|
||||
warnx("ARMING TEST: NORMAL OPERATION");
|
||||
@@ -232,7 +241,7 @@ int test_mixer(int argc, char *argv[])
|
||||
for (int j = -jmax; j <= jmax; j++) {
|
||||
|
||||
for (unsigned i = 0; i < output_max; i++) {
|
||||
actuator_controls[i] = j/10.0f + 0.1f * i;
|
||||
actuator_controls[i] = j / 10.0f + 0.1f * i;
|
||||
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
|
||||
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
|
||||
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
|
||||
@@ -241,12 +250,14 @@ int test_mixer(int argc, char *argv[])
|
||||
/* mix */
|
||||
mixed = mixer_group.mix(&outputs[0], output_max);
|
||||
|
||||
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
pwm_limit_calc(should_arm, mixed, 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)", mixed, output_max);
|
||||
for (unsigned i = 0; i < mixed; i++)
|
||||
{
|
||||
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
|
||||
|
||||
if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
|
||||
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
|
||||
warnx("mixer violated predicted value");
|
||||
@@ -262,15 +273,15 @@ int test_mixer(int argc, char *argv[])
|
||||
should_arm = false;
|
||||
|
||||
while (hrt_elapsed_time(&starttime) < 600000) {
|
||||
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_group.mix(&outputs[0], output_max);
|
||||
|
||||
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
pwm_limit_calc(should_arm, mixed, 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);
|
||||
for (unsigned i = 0; i < mixed; i++)
|
||||
{
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
/* check mixed outputs to be zero during init phase */
|
||||
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
|
||||
warnx("disarmed servo value mismatch");
|
||||
@@ -279,6 +290,7 @@ int test_mixer(int argc, char *argv[])
|
||||
|
||||
//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
|
||||
}
|
||||
|
||||
usleep(sleep_quantum_us);
|
||||
sleepcount++;
|
||||
|
||||
@@ -287,6 +299,7 @@ int test_mixer(int argc, char *argv[])
|
||||
fflush(stdout);
|
||||
}
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
|
||||
warnx("ARMING TEST: REARMING: STARTING RAMP");
|
||||
@@ -296,30 +309,30 @@ int test_mixer(int argc, char *argv[])
|
||||
should_arm = true;
|
||||
|
||||
while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
|
||||
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_group.mix(&outputs[0], output_max);
|
||||
|
||||
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
pwm_limit_calc(should_arm, mixed, 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);
|
||||
for (unsigned i = 0; i < mixed; i++)
|
||||
{
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
/* predict value */
|
||||
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
|
||||
|
||||
/* check ramp */
|
||||
|
||||
if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
|
||||
(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
|
||||
r_page_servos[i] > servo_predicted[i])) {
|
||||
(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
|
||||
r_page_servos[i] > servo_predicted[i])) {
|
||||
warnx("ramp servo value mismatch");
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* check post ramp phase */
|
||||
if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
|
||||
fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
|
||||
fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
|
||||
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
|
||||
warnx("mixer violated predicted value");
|
||||
return 1;
|
||||
@@ -327,6 +340,7 @@ int test_mixer(int argc, char *argv[])
|
||||
|
||||
//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
|
||||
}
|
||||
|
||||
usleep(sleep_quantum_us);
|
||||
sleepcount++;
|
||||
|
||||
@@ -335,15 +349,18 @@ int test_mixer(int argc, char *argv[])
|
||||
fflush(stdout);
|
||||
}
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
|
||||
/* load multirotor at once test */
|
||||
mixer_group.reset();
|
||||
|
||||
if (argc > 2)
|
||||
if (argc > 2) {
|
||||
filename = argv[2];
|
||||
else
|
||||
|
||||
} else {
|
||||
filename = "/etc/mixers/quad_w.main.mix";
|
||||
}
|
||||
|
||||
load_mixer_file(filename, &buf[0], sizeof(buf));
|
||||
loaded = strlen(buf);
|
||||
@@ -353,6 +370,7 @@ int test_mixer(int argc, char *argv[])
|
||||
unsigned mc_loaded = loaded;
|
||||
mixer_group.load_from_buf(&buf[0], mc_loaded);
|
||||
warnx("complete buffer load: loaded %u mixers", mixer_group.count());
|
||||
|
||||
if (mixer_group.count() != 5) {
|
||||
warnx("FAIL: Quad W mixer load failed");
|
||||
return 1;
|
||||
@@ -368,11 +386,13 @@ mixer_callback(uintptr_t handle,
|
||||
uint8_t control_index,
|
||||
float &control)
|
||||
{
|
||||
if (control_group != 0)
|
||||
if (control_group != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0])))
|
||||
if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0]))) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
control = actuator_controls[control_index];
|
||||
|
||||
|
||||
@@ -63,11 +63,12 @@ test_mount(int argc, char *argv[])
|
||||
const unsigned iterations = 2000;
|
||||
const unsigned alignments = 10;
|
||||
|
||||
const char* cmd_filename = "/fs/microsd/mount_test_cmds.txt";
|
||||
const char *cmd_filename = "/fs/microsd/mount_test_cmds.txt";
|
||||
|
||||
|
||||
/* check if microSD card is mounted */
|
||||
struct stat buffer;
|
||||
|
||||
if (stat("/fs/microsd/", &buffer)) {
|
||||
warnx("no microSD card mounted, aborting file test");
|
||||
return 1;
|
||||
@@ -77,6 +78,7 @@ test_mount(int argc, char *argv[])
|
||||
DIR *d;
|
||||
struct dirent *dir;
|
||||
d = opendir("/fs/microsd");
|
||||
|
||||
if (d) {
|
||||
|
||||
while ((dir = readdir(d)) != NULL) {
|
||||
@@ -105,6 +107,7 @@ test_mount(int argc, char *argv[])
|
||||
int it_left_abort = abort_tries;
|
||||
|
||||
int cmd_fd;
|
||||
|
||||
if (stat(cmd_filename, &buffer) == OK) {
|
||||
|
||||
/* command file exists, read off state */
|
||||
@@ -115,24 +118,28 @@ test_mount(int argc, char *argv[])
|
||||
if (ret > 0) {
|
||||
int count = 0;
|
||||
ret = sscanf(buf, "TEST: %u %u %n", &it_left_fsync, &it_left_abort, &count);
|
||||
|
||||
} else {
|
||||
buf[0] = '\0';
|
||||
}
|
||||
|
||||
if (it_left_fsync > fsync_tries)
|
||||
if (it_left_fsync > fsync_tries) {
|
||||
it_left_fsync = fsync_tries;
|
||||
}
|
||||
|
||||
if (it_left_abort > abort_tries)
|
||||
if (it_left_abort > abort_tries) {
|
||||
it_left_abort = abort_tries;
|
||||
}
|
||||
|
||||
warnx("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;
|
||||
|
||||
/* now write again what to do next */
|
||||
if (it_left_fsync > 0)
|
||||
if (it_left_fsync > 0) {
|
||||
it_left_fsync--;
|
||||
}
|
||||
|
||||
if (it_left_fsync == 0 && it_left_abort > 0) {
|
||||
|
||||
@@ -172,7 +179,8 @@ test_mount(int argc, char *argv[])
|
||||
|
||||
for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
|
||||
|
||||
printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC");
|
||||
printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c],
|
||||
(it_left_fsync > 0) ? "FSYNC" : "NO FSYNC");
|
||||
printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n");
|
||||
fsync(fileno(stdout));
|
||||
fsync(fileno(stderr));
|
||||
@@ -187,7 +195,7 @@ test_mount(int argc, char *argv[])
|
||||
/* fill write buffer with known values */
|
||||
for (unsigned i = 0; i < sizeof(write_buf); i++) {
|
||||
/* this will wrap, but we just need a known value with spacing */
|
||||
write_buf[i] = i+11;
|
||||
write_buf[i] = i + 11;
|
||||
}
|
||||
|
||||
uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
|
||||
@@ -201,8 +209,9 @@ test_mount(int argc, char *argv[])
|
||||
if (wret != (int)chunk_sizes[c]) {
|
||||
warn("WRITE ERROR!");
|
||||
|
||||
if ((0x3 & (uintptr_t)(write_buf + a)))
|
||||
if ((0x3 & (uintptr_t)(write_buf + a))) {
|
||||
warnx("memory is unaligned, align shift: %d", a);
|
||||
}
|
||||
|
||||
return 1;
|
||||
|
||||
@@ -210,6 +219,7 @@ test_mount(int argc, char *argv[])
|
||||
|
||||
if (it_left_fsync > 0) {
|
||||
fsync(fd);
|
||||
|
||||
} else {
|
||||
printf("#");
|
||||
fsync(fileno(stdout));
|
||||
@@ -237,7 +247,7 @@ test_mount(int argc, char *argv[])
|
||||
warnx("READ ERROR!");
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
/* compare value */
|
||||
bool compare_ok = true;
|
||||
|
||||
|
||||
@@ -77,6 +77,7 @@ int test_ppm_loopback(int argc, char *argv[])
|
||||
|
||||
unsigned servo_count;
|
||||
result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
|
||||
|
||||
if (result != OK) {
|
||||
warnx("PWM_SERVO_GET_COUNT");
|
||||
return ERROR;
|
||||
@@ -97,7 +98,7 @@ int test_ppm_loopback(int argc, char *argv[])
|
||||
// result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0);
|
||||
// if (result != OK)
|
||||
// warnx("FAIL: PWM_SERVO_SET_ARM_OK");
|
||||
// tell output device that the system is armed (it will output values if safety is off)
|
||||
// tell output device that the system is armed (it will output values if safety is off)
|
||||
// result = ioctl(servo_fd, PWM_SERVO_ARM, 0);
|
||||
// if (result != OK)
|
||||
// warnx("FAIL: PWM_SERVO_ARM");
|
||||
@@ -105,15 +106,17 @@ int test_ppm_loopback(int argc, char *argv[])
|
||||
int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400};
|
||||
|
||||
|
||||
for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
|
||||
result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]);
|
||||
if (result) {
|
||||
(void)close(servo_fd);
|
||||
return ERROR;
|
||||
} else {
|
||||
warnx("channel %d set to %d", i, pwm_values[i]);
|
||||
}
|
||||
}
|
||||
for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
|
||||
result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]);
|
||||
|
||||
if (result) {
|
||||
(void)close(servo_fd);
|
||||
return ERROR;
|
||||
|
||||
} else {
|
||||
warnx("channel %d set to %d", i, pwm_values[i]);
|
||||
}
|
||||
}
|
||||
|
||||
warnx("servo count: %d", servo_count);
|
||||
|
||||
@@ -165,6 +168,7 @@ int test_ppm_loopback(int argc, char *argv[])
|
||||
return ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
warnx("failed reading RC input data");
|
||||
return ERROR;
|
||||
|
||||
@@ -34,7 +34,7 @@
|
||||
/**
|
||||
* @file test_sensors.c
|
||||
* Tests the onboard sensors.
|
||||
*
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
@@ -62,10 +62,10 @@
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
|
||||
static int accel(int argc, char *argv[], const char* path);
|
||||
static int gyro(int argc, char *argv[], const char* path);
|
||||
static int mag(int argc, char *argv[], const char* path);
|
||||
static int baro(int argc, char *argv[], const char* path);
|
||||
static int accel(int argc, char *argv[], const char *path);
|
||||
static int gyro(int argc, char *argv[], const char *path);
|
||||
static int mag(int argc, char *argv[], const char *path);
|
||||
static int baro(int argc, char *argv[], const char *path);
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
@@ -74,7 +74,7 @@ static int baro(int argc, char *argv[], const char* path);
|
||||
struct {
|
||||
const char *name;
|
||||
const char *path;
|
||||
int (* test)(int argc, char *argv[], const char* path);
|
||||
int (* test)(int argc, char *argv[], const char *path);
|
||||
} sensors[] = {
|
||||
{"accel0", ACCEL0_DEVICE_PATH, accel},
|
||||
{"accel1", ACCEL1_DEVICE_PATH, accel},
|
||||
@@ -86,7 +86,7 @@ struct {
|
||||
};
|
||||
|
||||
static int
|
||||
accel(int argc, char *argv[], const char* path)
|
||||
accel(int argc, char *argv[], const char *path)
|
||||
{
|
||||
printf("\tACCEL: test start\n");
|
||||
fflush(stdout);
|
||||
@@ -136,7 +136,7 @@ accel(int argc, char *argv[], const char* path)
|
||||
}
|
||||
|
||||
static int
|
||||
gyro(int argc, char *argv[], const char* path)
|
||||
gyro(int argc, char *argv[], const char *path)
|
||||
{
|
||||
printf("\tGYRO: test start\n");
|
||||
fflush(stdout);
|
||||
@@ -181,7 +181,7 @@ gyro(int argc, char *argv[], const char* path)
|
||||
}
|
||||
|
||||
static int
|
||||
mag(int argc, char *argv[], const char* path)
|
||||
mag(int argc, char *argv[], const char *path)
|
||||
{
|
||||
printf("\tMAG: test start\n");
|
||||
fflush(stdout);
|
||||
@@ -226,7 +226,7 @@ mag(int argc, char *argv[], const char* path)
|
||||
}
|
||||
|
||||
static int
|
||||
baro(int argc, char *argv[], const char* path)
|
||||
baro(int argc, char *argv[], const char *path)
|
||||
{
|
||||
printf("\tBARO: test start\n");
|
||||
fflush(stdout);
|
||||
@@ -253,7 +253,8 @@ baro(int argc, char *argv[], const char* path)
|
||||
return ERROR;
|
||||
|
||||
} else {
|
||||
printf("\tBARO pressure: %8.4f mbar\talt: %8.4f m\ttemp: %8.4f deg C\n", (double)buf.pressure, (double)buf.altitude, (double)buf.temperature);
|
||||
printf("\tBARO pressure: %8.4f mbar\talt: %8.4f m\ttemp: %8.4f deg C\n", (double)buf.pressure, (double)buf.altitude,
|
||||
(double)buf.temperature);
|
||||
}
|
||||
|
||||
/* Let user know everything is ok */
|
||||
|
||||
@@ -81,6 +81,7 @@ int test_servo(int argc, char *argv[])
|
||||
|
||||
unsigned servo_count;
|
||||
result = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
|
||||
|
||||
if (result != OK) {
|
||||
warnx("PWM_SERVO_GET_COUNT");
|
||||
return ERROR;
|
||||
@@ -100,12 +101,17 @@ int test_servo(int argc, char *argv[])
|
||||
|
||||
/* tell safety that its ok to disable it with the switch */
|
||||
result = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
|
||||
if (result != OK)
|
||||
|
||||
if (result != OK) {
|
||||
warnx("FAIL: PWM_SERVO_SET_ARM_OK");
|
||||
}
|
||||
|
||||
/* tell output device that the system is armed (it will output values if safety is off) */
|
||||
result = ioctl(fd, PWM_SERVO_ARM, 0);
|
||||
if (result != OK)
|
||||
|
||||
if (result != OK) {
|
||||
warnx("FAIL: PWM_SERVO_ARM");
|
||||
}
|
||||
|
||||
usleep(5000000);
|
||||
printf("Advancing channel 0 to 1500\n");
|
||||
|
||||
@@ -90,8 +90,9 @@ cycletime(void)
|
||||
|
||||
cycles = *(unsigned long *)0xe0001004;
|
||||
|
||||
if (cycles < lasttime)
|
||||
if (cycles < lasttime) {
|
||||
basetime += 0x100000000ULL;
|
||||
}
|
||||
|
||||
lasttime = cycles;
|
||||
|
||||
@@ -147,11 +148,13 @@ int test_time(int argc, char *argv[])
|
||||
delta = abs(h - c);
|
||||
deltadelta = abs(delta - lowdelta);
|
||||
|
||||
if (deltadelta > maxdelta)
|
||||
if (deltadelta > maxdelta) {
|
||||
maxdelta = deltadelta;
|
||||
}
|
||||
|
||||
if (deltadelta > 1000)
|
||||
if (deltadelta > 1000) {
|
||||
fprintf(stderr, "h %llu c %llu d %lld\n", h, c, delta - lowdelta);
|
||||
}
|
||||
}
|
||||
|
||||
printf("Maximum jitter %lldus\n", maxdelta);
|
||||
|
||||
@@ -92,8 +92,9 @@ int test_uart_loopback(int argc, char *argv[])
|
||||
/* uart2 -> uart5 */
|
||||
r = write(uart2, sample_uart2, sizeof(sample_uart2));
|
||||
|
||||
if (r > 0)
|
||||
if (r > 0) {
|
||||
uart2_nwrite += r;
|
||||
}
|
||||
|
||||
// printf("TEST #%d\n",i);
|
||||
write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
|
||||
@@ -101,8 +102,9 @@ int test_uart_loopback(int argc, char *argv[])
|
||||
/* uart2 -> uart5 */
|
||||
r = write(uart5, sample_uart5, sizeof(sample_uart5));
|
||||
|
||||
if (r > 0)
|
||||
if (r > 0) {
|
||||
uart5_nwrite += r;
|
||||
}
|
||||
|
||||
// printf("TEST #%d\n",i);
|
||||
write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
|
||||
@@ -111,8 +113,9 @@ int test_uart_loopback(int argc, char *argv[])
|
||||
do {
|
||||
r = read(uart5, sample_uart2, sizeof(sample_uart2));
|
||||
|
||||
if (r > 0)
|
||||
if (r > 0) {
|
||||
uart5_nread += r;
|
||||
}
|
||||
} while (r > 0);
|
||||
|
||||
// printf("TEST #%d\n",i);
|
||||
@@ -121,8 +124,9 @@ int test_uart_loopback(int argc, char *argv[])
|
||||
do {
|
||||
r = read(uart2, sample_uart5, sizeof(sample_uart5));
|
||||
|
||||
if (r > 0)
|
||||
if (r > 0) {
|
||||
uart2_nread += r;
|
||||
}
|
||||
} while (r > 0);
|
||||
|
||||
// printf("TEST #%d\n",i);
|
||||
@@ -134,16 +138,19 @@ int test_uart_loopback(int argc, char *argv[])
|
||||
/* try to read back values */
|
||||
r = read(uart5, sample_uart2, sizeof(sample_uart2));
|
||||
|
||||
if (r > 0)
|
||||
if (r > 0) {
|
||||
uart5_nread += r;
|
||||
}
|
||||
|
||||
r = read(uart2, sample_uart5, sizeof(sample_uart5));
|
||||
|
||||
if (r > 0)
|
||||
if (r > 0) {
|
||||
uart2_nread += r;
|
||||
}
|
||||
|
||||
if ((uart2_nread == uart2_nwrite) && (uart5_nread == uart5_nwrite))
|
||||
if ((uart2_nread == uart2_nwrite) && (uart5_nread == uart5_nwrite)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -126,8 +126,9 @@ test_help(int argc, char *argv[])
|
||||
|
||||
printf("Available tests:\n");
|
||||
|
||||
for (i = 0; tests[i].name; i++)
|
||||
for (i = 0; tests[i].name; i++) {
|
||||
printf(" %s\n", tests[i].name);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -155,11 +156,13 @@ test_all(int argc, char *argv[])
|
||||
fflush(stderr);
|
||||
failcount++;
|
||||
passed[i] = false;
|
||||
|
||||
} else {
|
||||
printf(" [%s] \t\t\tPASS\n", tests[i].name);
|
||||
fflush(stdout);
|
||||
passed[i] = true;
|
||||
}
|
||||
|
||||
testcount++;
|
||||
}
|
||||
}
|
||||
@@ -198,7 +201,7 @@ test_all(int argc, char *argv[])
|
||||
printf("\n");
|
||||
|
||||
/* Print failed tests */
|
||||
if (failcount > 0) printf(" Failed tests:\n\n");
|
||||
if (failcount > 0) { printf(" Failed tests:\n\n"); }
|
||||
|
||||
unsigned int k;
|
||||
|
||||
@@ -254,22 +257,26 @@ int test_jig(int argc, char *argv[])
|
||||
bool passed[NTESTS];
|
||||
|
||||
printf("\nRunning all tests...\n\n");
|
||||
|
||||
for (i = 0; tests[i].name; i++) {
|
||||
/* Only run tests that are not excluded */
|
||||
if (!(tests[i].options & OPT_NOJIGTEST)) {
|
||||
printf(" [%s] \t\t\tSTARTING TEST\n", tests[i].name);
|
||||
fflush(stdout);
|
||||
|
||||
/* Execute test */
|
||||
if (tests[i].fn(1, args) != 0) {
|
||||
fprintf(stderr, " [%s] \t\t\tFAIL\n", tests[i].name);
|
||||
fflush(stderr);
|
||||
failcount++;
|
||||
passed[i] = false;
|
||||
|
||||
} else {
|
||||
printf(" [%s] \t\t\tPASS\n", tests[i].name);
|
||||
fflush(stdout);
|
||||
passed[i] = true;
|
||||
}
|
||||
|
||||
testcount++;
|
||||
}
|
||||
}
|
||||
@@ -277,13 +284,15 @@ int test_jig(int argc, char *argv[])
|
||||
/* Print summary */
|
||||
printf("\n");
|
||||
int j;
|
||||
for (j = 0; j < 40; j++)
|
||||
{
|
||||
|
||||
for (j = 0; j < 40; j++) {
|
||||
printf("-");
|
||||
}
|
||||
|
||||
printf("\n\n");
|
||||
|
||||
printf(" T E S T S U M M A R Y\n\n");
|
||||
|
||||
if (failcount == 0) {
|
||||
printf(" ______ __ __ ______ __ __ \n");
|
||||
printf(" /\\ __ \\ /\\ \\ /\\ \\ /\\ __ \\ /\\ \\/ / \n");
|
||||
@@ -292,6 +301,7 @@ int test_jig(int argc, char *argv[])
|
||||
printf(" \\/_/\\/_/ \\/_____/ \\/_____/ \\/_____/ \\/_/\\/_/ \n");
|
||||
printf("\n");
|
||||
printf(" All tests passed (%d of %d)\n", testcount, testcount);
|
||||
|
||||
} else {
|
||||
printf(" ______ ______ __ __ \n");
|
||||
printf(" /\\ ___\\ /\\ __ \\ /\\ \\ /\\ \\ \n");
|
||||
@@ -301,18 +311,20 @@ int test_jig(int argc, char *argv[])
|
||||
printf("\n");
|
||||
printf(" Some tests failed (%d of %d)\n", failcount, testcount);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
|
||||
/* Print failed tests */
|
||||
if (failcount > 0) printf(" Failed tests:\n\n");
|
||||
if (failcount > 0) { printf(" Failed tests:\n\n"); }
|
||||
|
||||
unsigned int k;
|
||||
for (k = 0; k < i; k++)
|
||||
{
|
||||
if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST))
|
||||
{
|
||||
|
||||
for (k = 0; k < i; k++) {
|
||||
if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST)) {
|
||||
printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
|
||||
}
|
||||
}
|
||||
|
||||
fflush(stdout);
|
||||
|
||||
return 0;
|
||||
@@ -333,8 +345,9 @@ int tests_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
for (i = 0; tests[i].name; i++) {
|
||||
if (!strcmp(tests[i].name, argv[1]))
|
||||
if (!strcmp(tests[i].name, argv[1])) {
|
||||
return tests[i].fn(argc - 1, argv + 1);
|
||||
}
|
||||
}
|
||||
|
||||
printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]);
|
||||
|
||||
Reference in New Issue
Block a user