From b5169b0c7bf4fcd75faa6acf5dc4055d53333554 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 20 Feb 2017 07:42:46 -0700 Subject: [PATCH] uncomment FMU servo readback test --- src/drivers/px4fmu/fmu.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 526fb71b14..3e3a89662d 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -3147,18 +3147,18 @@ test(void) } } -// /* 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]); -// } -// } + /* 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]); + } + } if (capture_count != 0 && (++rate_limit % 500 == 0)) { for (unsigned i = 0; i < capture_count; i++) {