diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/modules/systemlib/pwm_limit/pwm_limit.c | 30 | ||||
| -rw-r--r-- | src/modules/systemlib/pwm_limit/pwm_limit.h | 18 | ||||
| -rw-r--r-- | src/systemcmds/tests/module.mk | 3 | ||||
| -rw-r--r-- | src/systemcmds/tests/test_conv.cpp | 76 | ||||
| -rw-r--r-- | src/systemcmds/tests/test_mixer.cpp | 52 | ||||
| -rw-r--r-- | src/systemcmds/tests/test_rc.c | 4 | ||||
| -rw-r--r-- | src/systemcmds/tests/tests.h | 1 | ||||
| -rw-r--r-- | src/systemcmds/tests/tests_main.c | 1 |
8 files changed, 158 insertions, 27 deletions
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index cac3dc82a..992a7024b 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -47,7 +47,7 @@ void pwm_limit_init(pwm_limit_t *limit) { - limit->state = LIMIT_STATE_OFF; + limit->state = PWM_LIMIT_STATE_OFF; limit->time_armed = 0; return; } @@ -56,26 +56,26 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ { /* first evaluate state changes */ switch (limit->state) { - case LIMIT_STATE_OFF: + case PWM_LIMIT_STATE_OFF: if (armed) - limit->state = LIMIT_STATE_RAMP; + limit->state = PWM_LIMIT_STATE_RAMP; limit->time_armed = hrt_absolute_time(); break; - case LIMIT_STATE_INIT: + case PWM_LIMIT_STATE_INIT: if (!armed) - limit->state = LIMIT_STATE_OFF; + limit->state = PWM_LIMIT_STATE_OFF; else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US) - limit->state = LIMIT_STATE_RAMP; + limit->state = PWM_LIMIT_STATE_RAMP; break; - case LIMIT_STATE_RAMP: + case PWM_LIMIT_STATE_RAMP: if (!armed) - limit->state = LIMIT_STATE_OFF; + limit->state = PWM_LIMIT_STATE_OFF; else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US) - limit->state = LIMIT_STATE_ON; + limit->state = PWM_LIMIT_STATE_ON; break; - case LIMIT_STATE_ON: + case PWM_LIMIT_STATE_ON: if (!armed) - limit->state = LIMIT_STATE_OFF; + limit->state = PWM_LIMIT_STATE_OFF; break; default: break; @@ -86,14 +86,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ /* then set effective_pwm based on state */ switch (limit->state) { - case LIMIT_STATE_OFF: - case LIMIT_STATE_INIT: + case PWM_LIMIT_STATE_OFF: + case PWM_LIMIT_STATE_INIT: for (unsigned i=0; i<num_channels; i++) { effective_pwm[i] = disarmed_pwm[i]; output[i] = 0.0f; } break; - case LIMIT_STATE_RAMP: + case PWM_LIMIT_STATE_RAMP: progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; for (unsigned i=0; i<num_channels; i++) { @@ -120,7 +120,7 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ output[i] = (float)progress/10000.0f * output[i]; } break; - case LIMIT_STATE_ON: + case PWM_LIMIT_STATE_ON: for (unsigned i=0; i<num_channels; i++) { effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; /* effective_output stays the same */ diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h index 9974770be..bc8709398 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.h +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -46,6 +46,8 @@ #include <stdint.h> #include <stdbool.h> +__BEGIN_DECLS + /* * time for the ESCs to initialize * (this is not actually needed if PWM is sent right after boot) @@ -56,18 +58,18 @@ */ #define RAMP_TIME_US 2500000 +enum pwm_limit_state { + PWM_LIMIT_STATE_OFF = 0, + PWM_LIMIT_STATE_INIT, + PWM_LIMIT_STATE_RAMP, + PWM_LIMIT_STATE_ON +}; + typedef struct { - enum { - LIMIT_STATE_OFF = 0, - LIMIT_STATE_INIT, - LIMIT_STATE_RAMP, - LIMIT_STATE_ON - } state; + enum pwm_limit_state state; uint64_t time_armed; } pwm_limit_t; -__BEGIN_DECLS - __EXPORT void pwm_limit_init(pwm_limit_t *limit); __EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit); diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 68a080c77..576caff99 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -28,4 +28,5 @@ SRCS = test_adc.c \ tests_main.c \ test_param.c \ test_ppm_loopback.c \ - test_rc.c + test_rc.c \ + test_conv.cpp diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp new file mode 100644 index 000000000..50dece816 --- /dev/null +++ b/src/systemcmds/tests/test_conv.cpp @@ -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; +} diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 4da86042d..d330da39f 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -51,6 +51,7 @@ #include <systemlib/err.h> #include <systemlib/mixer/mixer.h> +#include <systemlib/pwm_limit/pwm_limit.h> #include "tests.h" @@ -59,6 +60,18 @@ 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]; +/* + * PWM limit structure + */ +pwm_limit_t pwm_limit; + int test_mixer(int argc, char *argv[]) { warnx("testing mixer"); @@ -164,6 +177,40 @@ int test_mixer(int argc, char *argv[]) if (mixer_group.count() != 8) return 1; + /* execute the mixer */ + + float outputs_unlimited[output_max]; + float outputs[output_max]; + unsigned mixed; + const int jmax = 50; + + pwm_limit_init(&pwm_limit); + pwm_limit.state = PWM_LIMIT_STATE_ON; + should_arm = true; + + for (int j = -jmax; j <= jmax; j++) { + + for (int i = 0; i < output_max; i++) { + actuator_controls[i] = j/100.0f + 0.1f * i; + r_page_servo_disarmed[i] = 900; + r_page_servo_control_min[i] = 1000; + r_page_servo_control_max[i] = 2000; + } + + /* mix */ + mixed = mixer_group.mix(&outputs_unlimited[0], output_max); + + memcpy(outputs, outputs_unlimited, sizeof(outputs)); + + 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++) + { + printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + } + /* load multirotor at once test */ mixer_group.reset(); @@ -193,7 +240,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; } diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 72619fc8b..6a602ecfc 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file test_ppm_loopback.c - * Tests the PWM outputs and PPM input + * @file test_rc.c + * Tests RC input. * */ diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index a57d04be3..321e91e60 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -109,6 +109,7 @@ 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[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 1088a4407..1c717d3ea 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -106,6 +106,7 @@ 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}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; |