diff options
author | px4dev <px4@purgatory.org> | 2013-01-26 14:36:31 -0800 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-01-26 14:36:31 -0800 |
commit | 2a18d6466c36c50244851d225a5319207b08b0bf (patch) | |
tree | 70041584416d39c5cb5d3438c7698c5bb1704e47 /apps | |
parent | 6ba4cd04fecd8600dbea7b94ccf0706eac40a089 (diff) | |
download | px4-firmware-2a18d6466c36c50244851d225a5319207b08b0bf.tar.gz px4-firmware-2a18d6466c36c50244851d225a5319207b08b0bf.tar.bz2 px4-firmware-2a18d6466c36c50244851d225a5319207b08b0bf.zip |
Add a bus saturation test for px4io.
Diffstat (limited to 'apps')
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 63 |
1 files changed, 43 insertions, 20 deletions
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2d7495aaa..2a860e5c1 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1112,34 +1112,57 @@ void test(void) { int fd; + unsigned servo_count = 0; + unsigned pwm_value = 1000; + int direction = 1; + int ret; - fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + fd = open("/dev/px4io", O_WRONLY); - if (fd < 0) { - puts("open fail"); - exit(1); - } + if (fd < 0) + err(1, "failed to open device"); - ioctl(fd, PWM_SERVO_ARM, 0); - ioctl(fd, PWM_SERVO_SET(0), 1000); - ioctl(fd, PWM_SERVO_SET(1), 1100); - ioctl(fd, PWM_SERVO_SET(2), 1200); - ioctl(fd, PWM_SERVO_SET(3), 1300); - ioctl(fd, PWM_SERVO_SET(4), 1400); - ioctl(fd, PWM_SERVO_SET(5), 1500); - ioctl(fd, PWM_SERVO_SET(6), 1600); - ioctl(fd, PWM_SERVO_SET(7), 1700); + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) + err(1, "failed to get servo count"); - close(fd); + if (ioctl(fd, PWM_SERVO_ARM, 0)) + err(1, "failed to arm servos"); - actuator_armed_s aa; + for (;;) { - aa.armed = true; - aa.lockdown = false; + /* sweep all servos between 1000..2000 */ + servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) + servos[i] = pwm_value; - orb_advertise(ORB_ID(actuator_armed), &aa); + ret = write(fd, servos, sizeof(servos)); + if (ret != sizeof(servos)) + err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); - exit(0); + if (direction > 0) { + if (pwm_value < 2000) { + pwm_value++; + } else { + direction = -1; + } + } else { + if (pwm_value > 1000) { + pwm_value--; + } else { + direction = 1; + } + } + + /* readback servo values */ + for (unsigned i = 0; i < servo_count; i++) { + servo_position_t value; + + if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) + err(1, "error reading PWM servo %d", i); + if (value != servos[i]) + errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); + } + } } void |