From 3ebea78d918b0fbf428f011c12e536765b0a735a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 May 2016 18:05:24 +0200 Subject: [PATCH] IO driver: Fix PWM load --- src/drivers/px4io/px4io.cpp | 58 ++++++++++++++++++++++--------------- 1 file changed, 35 insertions(+), 23 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 000d04e491..2b4c41d7ae 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -687,7 +687,7 @@ PX4IO::init() _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); - if ((_max_actuators < 1) || (_max_actuators > 255) || + if ((_max_actuators < 1) || (_max_actuators > 16) || (_max_relays > 32) || (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { @@ -2475,16 +2475,19 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case PWM_SERVO_GET_FAILSAFE_PWM: + case PWM_SERVO_GET_FAILSAFE_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + pwm->channel_count = _max_actuators; - ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t *)arg, _max_actuators); + ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, _max_actuators); - if (ret != OK) { - ret = -EIO; + if (ret != OK) { + ret = -EIO; + } + + break; } - break; - case PWM_SERVO_SET_DISARMED_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; @@ -2499,16 +2502,19 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case PWM_SERVO_GET_DISARMED_PWM: + case PWM_SERVO_GET_DISARMED_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + pwm->channel_count = _max_actuators; - ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t *)arg, _max_actuators); + ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, _max_actuators); - if (ret != OK) { - ret = -EIO; + if (ret != OK) { + ret = -EIO; + } + + break; } - break; - case PWM_SERVO_SET_MIN_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; @@ -2523,16 +2529,19 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case PWM_SERVO_GET_MIN_PWM: + case PWM_SERVO_GET_MIN_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + pwm->channel_count = _max_actuators; - ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t *)arg, _max_actuators); + ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, _max_actuators); - if (ret != OK) { - ret = -EIO; + if (ret != OK) { + ret = -EIO; + } + + break; } - break; - case PWM_SERVO_SET_MAX_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; @@ -2547,12 +2556,15 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case PWM_SERVO_GET_MAX_PWM: + case PWM_SERVO_GET_MAX_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + pwm->channel_count = _max_actuators; - ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t *)arg, _max_actuators); + ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, _max_actuators); - if (ret != OK) { - ret = -EIO; + if (ret != OK) { + ret = -EIO; + } } break;