fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 07:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged beta into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-07-09 15:55:31 +02:00
parent b1b555ceb6 3fa7006576
commit 2a8402edb1
15 changed files with 326 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/systemcmds/tests/test_mixer.cpp
+58 -16
View File
@@ -56,6 +56,8 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <uORB/topics/actuator_controls.h>
#include "tests.h"
static int mixer_callback(uintptr_t handle,
@@ -65,6 +67,9 @@ static int mixer_callback(uintptr_t handle,
const unsigned output_max = 8;
static float actuator_controls[output_max];
static bool should_prearm = false;
#define NAN_VALUE 0.0f/0.0f
int test_mixer(int argc, char *argv[])
{
@@ -72,7 +77,7 @@ int test_mixer(int argc, char *argv[])
* PWM limit structure
*/
pwm_limit_t pwm_limit;
static bool should_arm = false;
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];
@@ -184,7 +189,6 @@ int test_mixer(int argc, char *argv[])
const int jmax = 5;
pwm_limit_init(&pwm_limit);
should_arm = true;
/* run through arming phase */
for (unsigned i = 0; i < output_max; i++) {
@@ -194,6 +198,40 @@ int test_mixer(int argc, char *argv[])
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
/* mix */
should_prearm = true;
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);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
warnx("pre-arm:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]);
if (i != actuator_controls_s::INDEX_THROTTLE) {
if (r_page_servos[i] < r_page_servo_control_min[i]) {
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!)");
return 1;
}
}
}
should_arm = true;
should_prearm = false;
/* simulate another orb_copy() from actuator controls */
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
}
PX4_INFO("ARMING TEST: STARTING RAMP");
unsigned sleep_quantum_us = 10000;
@@ -205,15 +243,18 @@ int test_mixer(int argc, char *argv[])
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
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);
for (unsigned i = 0; i < mixed; i++) {
warnx("ramp:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[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]) {
PX4_ERR("disarmed servo value mismatch");
PX4_ERR("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]);
return 1;
}
@@ -222,21 +263,16 @@ int test_mixer(int argc, char *argv[])
PX4_ERR("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++;
if (sleepcount % 10 == 0) {
printf(".");
fflush(stdout);
}
}
printf("\n");
PX4_INFO("ARMING TEST: NORMAL OPERATION");
for (int j = -jmax; j <= jmax; j++) {
@@ -251,7 +287,7 @@ int test_mixer(int argc, char *argv[])
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, 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);
@@ -278,18 +314,19 @@ int test_mixer(int argc, char *argv[])
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, 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);
for (unsigned i = 0; i < mixed; i++) {
warnx("disarmed:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]);
/* check mixed outputs to be zero during init phase */
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
PX4_ERR("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);
@@ -314,7 +351,7 @@ int test_mixer(int argc, char *argv[])
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, 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);
@@ -324,6 +361,8 @@ int test_mixer(int argc, char *argv[])
/* check ramp */
warnx("ramp:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]);
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])) {
@@ -338,8 +377,6 @@ int test_mixer(int argc, char *argv[])
PX4_ERR("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);
@@ -397,5 +434,10 @@ 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 = NAN_VALUE;
}
return 0;
}