yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

PPM loopback test

Browse Source
This commit is contained in:
Lorenz Meier
2013-12-05 05:02:00 +01:00
parent 1cb576ae4e
commit 264ef47197
1 changed files with 66 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
src/systemcmds/tests/test_ppm_loopback.c
View File

@@ -51,6 +51,7 @@
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_rc_input.h>
#include <uORB/topics/rc_channels.h>
#include <systemlib/err.h>
#include "tests.h"
@@ -61,6 +62,8 @@
int test_ppm_loopback(int argc, char *argv[])
{
int _rc_sub = orb_subscribe(ORB_ID(input_rc));
int servo_fd, result;
servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
servo_position_t pos;
@@ -71,12 +74,6 @@ int test_ppm_loopback(int argc, char *argv[])
printf("failed opening /dev/pwm_servo\n");
}
result = read(servo_fd, &data, sizeof(data));
if (result != sizeof(data)) {
printf("failed bulk-reading channel values\n");
}
printf("Servo readback, pairs of values should match defaults\n");
unsigned servo_count;
@@ -93,62 +90,89 @@ int test_ppm_loopback(int argc, char *argv[])
printf("failed reading channel %u\n", i);
}
printf("%u: %u %u\n", i, pos, data[i]);
//printf("%u: %u %u\n", i, pos, data[i]);
}
/* tell safety that its ok to disable it with the switch */
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) */
result = ioctl(servo_fd, PWM_SERVO_ARM, 0);
if (result != OK)
warnx("FAIL: PWM_SERVO_ARM");
// /* tell safety that its ok to disable it with the switch */
// 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)
// result = ioctl(servo_fd, PWM_SERVO_ARM, 0);
// if (result != OK)
// warnx("FAIL: PWM_SERVO_ARM");
int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400};
printf("Advancing channel 0 to 1100\n");
result = ioctl(servo_fd, PWM_SERVO_SET(0), 1100);
printf("Advancing channel 1 to 1900\n");
result = ioctl(servo_fd, PWM_SERVO_SET(1), 1900);
printf("Advancing channel 2 to 1200\n");
result = ioctl(servo_fd, PWM_SERVO_SET(2), 1200);
// 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);
struct pwm_output_values pwm_out = {.values = {0}, .channel_count = 0};
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;
}
pwm_out.values[i] = pwm_values[i];
//warnx("channel %d: disarmed PWM: %d", i+1, pwm_values[i]);
pwm_out.channel_count++;
}
result = ioctl(servo_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_out);
/* give driver 10 ms to propagate */
usleep(10000);
/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
struct rc_input_values rc_input;
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
usleep(100000);
/* open PPM input and expect values close to the output values */
int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY);
bool rc_updated;
orb_check(_rc_sub, &rc_updated);
struct rc_input_values rc;
result = read(ppm_fd, &rc, sizeof(rc));
if (rc_updated) {
if (result != sizeof(rc)) {
warnx("Error reading RC output");
(void)close(servo_fd);
(void)close(ppm_fd);
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
// int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY);
// struct rc_input_values rc;
// result = read(ppm_fd, &rc, sizeof(rc));
// if (result != sizeof(rc)) {
// warnx("Error reading RC output");
// (void)close(servo_fd);
// (void)close(ppm_fd);
// return ERROR;
// }
/* go and check values */
for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
if (fabsf(rc_input.values[i] - pwm_values[i]) > 10) {
warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]);
(void)close(servo_fd);
return ERROR;
}
}
} else {
warnx("failed reading RC input data");
return ERROR;
}
/* go and check values */
for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
result = ioctl(servo_fd, PWM_SERVO_GET(i), pwm_values[i]);
if (result) {
(void)close(servo_fd);
return ERROR;
}
}
warnx("PPM LOOPBACK TEST PASSED SUCCESSFULLY!");
return 0;
}