diff options
author | Andrew Tridgell <tridge@samba.org> | 2013-07-28 17:20:12 +1000 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-02 12:44:47 +0200 |
commit | f3764d0b5a219aea24d05274311d91ad08eb182d (patch) | |
tree | e06d1212b4ae41a82ca06782f68f487bf649dd51 | |
parent | feee1ccc65dff865270d22ff6796b6acedf67ea2 (diff) | |
download | px4-firmware-f3764d0b5a219aea24d05274311d91ad08eb182d.tar.gz px4-firmware-f3764d0b5a219aea24d05274311d91ad08eb182d.tar.bz2 px4-firmware-f3764d0b5a219aea24d05274311d91ad08eb182d.zip |
px4fmu: expanded "fmu test"
this now does testing in a similar manner as "px4io test", except that
it tests both ioctl and write based setting of servos
-rw-r--r-- | src/drivers/px4fmu/fmu.cpp | 84 |
1 files changed, 70 insertions, 14 deletions
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 70147d56a..b73f71572 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1104,28 +1104,84 @@ void test(void) { int fd; - - fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + unsigned servo_count = 0; + unsigned pwm_value = 1000; + int direction = 1; + + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); if (fd < 0) errx(1, "open fail"); if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); - if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); - - if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); - - if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) { + err(1, "Unable to get servo count\n"); + } + + warnx("Testing %u servos", (unsigned)servo_count); + + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + err(1, "failed opening console"); + + warnx("Press CTRL-C or 'c' to abort."); + + for (;;) { + /* sweep all servos between 1000..2000 */ + servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) + servos[i] = pwm_value; + + if (direction == 1) { + // use ioctl interface for one direction + for (unsigned i=0; i < servo_count; i++) { + if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) { + err(1, "servo %u set failed", i); + } + } + } else { + // and use write interface for the other direction + int ret = write(fd, servos, sizeof(servos)); + if (ret != (int)sizeof(servos)) + err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); + } + + if (direction > 0) { + if (pwm_value < 2000) { + pwm_value++; + } else { + direction = -1; + } + } else { + if (pwm_value > 1000) { + pwm_value--; + } else { + direction = 1; + } + } - if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); + /* readback servo values */ + for (unsigned i = 0; i < servo_count; i++) { + servo_position_t value; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - if (ioctl(fd, PWM_SERVO_SET(4), 1800) < 0) err(1, "servo 5 set failed"); + 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]); + } - if (ioctl(fd, PWM_SERVO_SET(5), 2000) < 0) err(1, "servo 6 set failed"); -#endif + /* Check if user wants to quit */ + char c; + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + warnx("User abort\n"); + break; + } + } + } + close(console); close(fd); exit(0); @@ -1222,9 +1278,9 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, "FMU: unrecognised command, try:\n"); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n"); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - fprintf(stderr, " mode_gpio, mode_pwm\n"); + fprintf(stderr, " mode_gpio, mode_pwm, test\n"); #endif exit(1); } |