Merge branch 'navigator_new' into navigator_new_vector

This commit is contained in:
Anton Babushkin
2014-01-17 14:37:48 +01:00
31 changed files with 1028 additions and 907 deletions
+3 -1
View File
@@ -30,4 +30,6 @@ SRCS = test_adc.c \
test_param.c \
test_ppm_loopback.c \
test_rc.c \
test_mount.c
test_conv.cpp \
test_mount.c \
test_mtd.c
+76
View File
@@ -0,0 +1,76 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file test_conv.cpp
* Tests conversions used across the system.
*
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdio.h>
#include <poll.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include "tests.h"
#include <math.h>
#include <float.h>
#include <systemlib/err.h>
#include <unit_test/unit_test.h>
#include <px4iofirmware/protocol.h>
int test_conv(int argc, char *argv[])
{
warnx("Testing system conversions");
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", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres);
return 1;
}
}
warnx("All conversions clean");
return 0;
}
+1 -1
View File
@@ -54,7 +54,7 @@
#include "tests.h"
int check_user_abort(int fd);
static int check_user_abort(int fd);
int check_user_abort(int fd) {
/* check if user wants to abort */
+195 -2
View File
@@ -48,9 +48,13 @@
#include <errno.h>
#include <string.h>
#include <time.h>
#include <math.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include "tests.h"
@@ -59,6 +63,20 @@ static int mixer_callback(uintptr_t handle,
uint8_t control_index,
float &control);
const unsigned output_max = 8;
static float actuator_controls[output_max];
static bool should_arm = false;
uint16_t r_page_servo_disarmed[output_max];
uint16_t r_page_servo_control_min[output_max];
uint16_t r_page_servo_control_max[output_max];
uint16_t r_page_servos[output_max];
uint16_t servo_predicted[output_max];
/*
* PWM limit structure
*/
pwm_limit_t pwm_limit;
int test_mixer(int argc, char *argv[])
{
warnx("testing mixer");
@@ -164,6 +182,174 @@ int test_mixer(int argc, char *argv[])
if (mixer_group.count() != 8)
return 1;
/* execute the mixer */
float outputs[output_max];
unsigned mixed;
const int jmax = 5;
pwm_limit_init(&pwm_limit);
should_arm = true;
/* run through arming phase */
for (int i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
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;
}
warnx("ARMING TEST: STARTING RAMP");
unsigned sleep_quantum_us = 10000;
hrt_abstime starttime = hrt_absolute_time();
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);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (int 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]) {
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]) {
warnx("ramp servo value mismatch");
return 1;
}
//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++;
if (sleepcount % 10 == 0) {
printf(".");
fflush(stdout);
}
}
printf("\n");
warnx("ARMING TEST: NORMAL OPERATION");
for (int j = -jmax; j <= jmax; j++) {
for (int i = 0; i < output_max; 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;
}
/* 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);
warnx("mixed %d outputs (max %d)", mixed, output_max);
for (int 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, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
warnx("mixer violated predicted value");
return 1;
}
}
}
warnx("ARMING TEST: DISARMING");
starttime = hrt_absolute_time();
sleepcount = 0;
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);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (int 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");
return 1;
}
//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++;
if (sleepcount % 10 == 0) {
printf(".");
fflush(stdout);
}
}
printf("\n");
warnx("ARMING TEST: REARMING: STARTING RAMP");
starttime = hrt_absolute_time();
sleepcount = 0;
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);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (int 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])) {
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) {
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
warnx("mixer violated predicted value");
return 1;
}
//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++;
if (sleepcount % 10 == 0) {
printf(".");
fflush(stdout);
}
}
printf("\n");
/* load multirotor at once test */
mixer_group.reset();
@@ -180,8 +366,12 @@ 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() != 8)
if (mixer_group.count() != 5) {
warnx("FAIL: Quad W mixer load failed");
return 1;
}
warnx("SUCCESS: No errors in mixer test");
}
static int
@@ -193,7 +383,10 @@ mixer_callback(uintptr_t handle,
if (control_group != 0)
return -1;
control = 0.0f;
if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0])))
return -1;
control = actuator_controls[control_index];
return 0;
}
+229
View File
@@ -0,0 +1,229 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file test_mtd.c
*
* Param storage / file test.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <sys/stat.h>
#include <poll.h>
#include <dirent.h>
#include <stdio.h>
#include <stddef.h>
#include <unistd.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include "tests.h"
#define PARAM_FILE_NAME "/fs/mtd_params"
static int check_user_abort(int fd);
int check_user_abort(int fd) {
/* check if user wants to abort */
char c;
struct pollfd fds;
int ret;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
switch (c) {
case 0x03: // ctrl-c
case 0x1b: // esc
case 'c':
case 'q':
{
warnx("Test aborted.");
fsync(fd);
close(fd);
return OK;
/* not reached */
}
}
}
return 1;
}
void print_fail()
{
printf("<[T]: MTD: FAIL>\n");
}
void print_success()
{
printf("<[T]: MTD: OK>\n");
}
int
test_mtd(int argc, char *argv[])
{
unsigned iterations= 0;
/* check if microSD card is mounted */
struct stat buffer;
if (stat(PARAM_FILE_NAME, &buffer)) {
warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME);
print_fail();
return 1;
}
// XXX get real storage space here
unsigned file_size = 4096;
/* perform tests for a range of chunk sizes */
unsigned chunk_sizes[] = {256, 512, 4096};
for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]);
uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64)));
/* fill write buffer with known values */
for (int i = 0; i < sizeof(write_buf); i++) {
/* this will wrap, but we just need a known value with spacing */
write_buf[i] = i+11;
}
uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64)));
hrt_abstime start, end;
int fd = open(PARAM_FILE_NAME, O_RDONLY);
int rret = read(fd, read_buf, chunk_sizes[c]);
close(fd);
fd = open(PARAM_FILE_NAME, O_WRONLY);
printf("printing 2 percent of the first chunk:\n");
for (int i = 0; i < sizeof(read_buf) / 50; i++) {
printf("%02X", read_buf[i]);
}
printf("\n");
iterations = file_size / chunk_sizes[c];
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
int wret = write(fd, write_buf, chunk_sizes[c]);
if (wret != (int)chunk_sizes[c]) {
warn("WRITE ERROR!");
print_fail();
return 1;
}
fsync(fd);
if (!check_user_abort(fd))
return OK;
}
end = hrt_absolute_time();
close(fd);
fd = open(PARAM_FILE_NAME, O_RDONLY);
/* read back data for validation */
for (unsigned i = 0; i < iterations; i++) {
int rret = read(fd, read_buf, chunk_sizes[c]);
if (rret != chunk_sizes[c]) {
warnx("READ ERROR!");
print_fail();
return 1;
}
/* compare value */
bool compare_ok = true;
for (int j = 0; j < chunk_sizes[c]; j++) {
if (read_buf[j] != write_buf[j]) {
warnx("COMPARISON ERROR: byte %d", j);
print_fail();
compare_ok = false;
break;
}
}
if (!compare_ok) {
warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
print_fail();
return 1;
}
if (!check_user_abort(fd))
return OK;
}
close(fd);
}
/* fill the file with 0xFF to make it look new again */
char ffbuf[64];
memset(ffbuf, 0xFF, sizeof(ffbuf));
int fd = open(PARAM_FILE_NAME, O_WRONLY);
for (int i = 0; i < file_size / sizeof(ffbuf); i++) {
int ret = write(fd, ffbuf, sizeof(ffbuf));
if (ret != sizeof(ffbuf)) {
warnx("ERROR! Could not fill file with 0xFF");
close(fd);
print_fail();
return 1;
}
}
(void)close(fd);
print_success();
return 0;
}
+2 -2
View File
@@ -32,8 +32,8 @@
****************************************************************************/
/**
* @file test_ppm_loopback.c
* Tests the PWM outputs and PPM input
* @file test_rc.c
* Tests RC input.
*
*/
+2
View File
@@ -109,7 +109,9 @@ extern int test_bson(int argc, char *argv[]);
extern int test_file(int argc, char *argv[]);
extern int test_mixer(int argc, char *argv[]);
extern int test_rc(int argc, char *argv[]);
extern int test_conv(int argc, char *argv[]);
extern int test_mount(int argc, char *argv[]);
extern int test_mtd(int argc, char *argv[]);
extern int test_mathlib(int argc, char *argv[]);
__END_DECLS
+2
View File
@@ -106,7 +106,9 @@ const struct {
{"file", test_file, 0},
{"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST},
{"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST},
{"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST},
{"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST},
{"mtd", test_mtd, 0},
{"mathlib", test_mathlib, 0},
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
{NULL, NULL, 0}