From 463b8a752838cb0cd39a8496578c4e7ba1ada448 Mon Sep 17 00:00:00 2001 From: Roman Date: Mon, 15 Apr 2019 21:51:57 +0200 Subject: [PATCH] vtol_att_control: respect min pwm values Signed-off-by: Roman --- src/modules/vtol_att_control/vtol_type.cpp | 25 ++++++++++++++++++++-- src/modules/vtol_att_control/vtol_type.h | 1 + 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 5d4b2c2188..53aabcdb85 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -95,6 +95,13 @@ bool VtolType::init() return false; } + ret = px4_ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&_min_mc_pwm_values); + + if (ret != PX4_OK) { + PX4_ERR("failed getting min values"); + px4_close(fd); + return false; + } ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&_disarmed_pwm_values); @@ -104,6 +111,8 @@ bool VtolType::init() return false; } + px4_close(fd); + return true; } @@ -240,7 +249,13 @@ bool VtolType::set_idle_mc() memset(&pwm_values, 0, sizeof(pwm_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { - pwm_values.values[i] = pwm_value; + if (is_motor_off_channel(i)) { + pwm_values.values[i] = pwm_value; + + } else { + pwm_values.values[i] = _min_mc_pwm_values.values[i]; + } + pwm_values.channel_count++; } @@ -254,7 +269,13 @@ bool VtolType::set_idle_fw() memset(&pwm_values, 0, sizeof(pwm_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { - pwm_values.values[i] = PWM_MOTOR_OFF; + if (is_motor_off_channel(i)) { + pwm_values.values[i] = PWM_MOTOR_OFF; + + } else { + pwm_values.values[i] = _min_mc_pwm_values.values[i]; + } + pwm_values.channel_count++; } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 68b25f255f..e73d9cb999 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -252,6 +252,7 @@ private: /** * @brief Stores the max pwm values given by the system. */ + struct pwm_output_values _min_mc_pwm_values {}; struct pwm_output_values _max_mc_pwm_values {}; struct pwm_output_values _disarmed_pwm_values {};