From 00f98c64dbc150aee5507cbc3bbdc4bf6b3064ae Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 5 Mar 2018 16:02:22 -0500 Subject: [PATCH] VTOL preserve PWM min and max when enabling/disabling actuators --- src/modules/vtol_att_control/standard.cpp | 50 +----- src/modules/vtol_att_control/standard.h | 6 +- src/modules/vtol_att_control/tailsitter.cpp | 9 +- src/modules/vtol_att_control/tiltrotor.cpp | 21 +-- src/modules/vtol_att_control/tiltrotor.h | 2 +- .../vtol_att_control_main.cpp | 3 - src/modules/vtol_att_control/vtol_type.cpp | 153 ++++++++++-------- src/modules/vtol_att_control/vtol_type.h | 8 + 8 files changed, 113 insertions(+), 139 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index c489764a67..53f4d0f957 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -48,7 +48,6 @@ Standard::Standard(VtolAttitudeControl *attc) : VtolType(attc), - _flag_enable_mc_motors(true), _pusher_throttle(0.0f), _reverse_output(0.0f), _airspeed_trans_blend_margin(0.0f) @@ -300,11 +299,9 @@ void Standard::update_transition_state() } - // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + // in back transition we need to start the MC motors again if (_flag_enable_mc_motors) { - set_max_mc(2000); - set_idle_mc(); - _flag_enable_mc_motors = false; + _flag_enable_mc_motors = !enable_mc_motors(); } } @@ -322,9 +319,7 @@ void Standard::update_mc_state() // enable MC motors here in case we transitioned directly to MC mode if (_flag_enable_mc_motors) { - set_max_mc(2000); - set_idle_mc(); - _flag_enable_mc_motors = false; + _flag_enable_mc_motors = !enable_mc_motors(); } // if the thrust scale param is zero or the drone is on manual mode, @@ -403,11 +398,9 @@ void Standard::update_fw_state() { VtolType::update_fw_state(); - // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + // stop MC motors in FW mode if (!_flag_enable_mc_motors) { - set_max_mc(950); - set_idle_fw(); // force them to stop, not just idle - _flag_enable_mc_motors = true; + _flag_enable_mc_motors = disable_mc_motors(); } } @@ -495,36 +488,3 @@ Standard::waiting_on_tecs() // keep thrust from transition _v_att_sp->thrust = _pusher_throttle; }; - -/** -* Disable all multirotor motors when in fw mode. -*/ -void -Standard::set_max_mc(unsigned pwm_value) -{ - int ret; - unsigned servo_count; - const char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = px4_open(dev, 0); - - if (fd < 0) { - PX4_WARN("can't open %s", dev); - } - - ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (int i = 0; i < _params->vtol_motor_count; i++) { - pwm_values.values[i] = pwm_value; - pwm_values.channel_count = _params->vtol_motor_count; - } - - ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) { - PX4_WARN("failed setting max values"); - } - - px4_close(fd); -} diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index bed2de9396..ad01f90ee7 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -98,13 +98,13 @@ private: hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) } _vtol_schedule; - bool _flag_enable_mc_motors; + // TODO: replace with vtol_type's flag_idle_mc + bool _flag_enable_mc_motors{true}; + float _pusher_throttle; float _reverse_output; float _airspeed_trans_blend_margin; - void set_max_mc(unsigned pwm_value); - virtual void parameters_update(); }; #endif diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index f3af043a5f..d6784214e4 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -214,8 +214,7 @@ void Tailsitter::update_transition_state() } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { if (!flag_idle_mc) { - set_idle_mc(); - flag_idle_mc = true; + flag_idle_mc = enable_mc_motors(); } // create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value @@ -261,8 +260,7 @@ void Tailsitter::update_mc_state() // set idle speed for rotary wing mode if (!flag_idle_mc) { - set_idle_mc(); - flag_idle_mc = true; + flag_idle_mc = enable_mc_motors(); } } @@ -271,8 +269,7 @@ void Tailsitter::update_fw_state() VtolType::update_fw_state(); if (flag_idle_mc) { - set_idle_fw(); - flag_idle_mc = false; + flag_idle_mc = !disable_mc_motors(); } } diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 73322d7a2a..8a81eea341 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -104,7 +104,7 @@ Tiltrotor::parameters_update() _params_tiltrotor.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f); } -int Tiltrotor::get_motor_off_channels(int channels) +uint32_t Tiltrotor::get_motor_off_channels(int channels) { int channel_bitmap = 0; @@ -251,8 +251,7 @@ void Tiltrotor::update_mc_state() // set idle speed for rotary wing mode if (!flag_idle_mc) { - set_idle_mc(); - flag_idle_mc = true; + flag_idle_mc = enable_mc_motors(); } } @@ -270,8 +269,7 @@ void Tiltrotor::update_fw_state() // adjust idle for fixed wing flight if (flag_idle_mc) { - set_idle_fw(); - flag_idle_mc = false; + flag_idle_mc = !disable_mc_motors(); } } @@ -347,8 +345,7 @@ void Tiltrotor::update_transition_state() } if (!flag_idle_mc) { - set_idle_mc(); - flag_idle_mc = true; + flag_idle_mc = enable_mc_motors(); } // tilt rotors back @@ -447,8 +444,6 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state, int value) break; } - int ret; - unsigned servo_count; const char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); @@ -456,9 +451,7 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state, int value) PX4_WARN("can't open %s", dev); } - ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - struct pwm_output_values pwm_max_values; - memset(&pwm_max_values, 0, sizeof(pwm_max_values)); + struct pwm_output_values pwm_max_values = {}; for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { @@ -471,10 +464,10 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state, int value) pwm_max_values.channel_count = _params->vtol_motor_count; } - ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values); + int ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values); if (ret != OK) { - PX4_WARN("failed setting max values"); + PX4_ERR("failed setting max values"); } px4_close(fd); diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 12a2cb0d17..25f5e16955 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -111,7 +111,7 @@ private: /** * Return a bitmap of channels that should be turned off in fixed wing mode. */ - int get_motor_off_channels(const int channels); + uint32_t get_motor_off_channels(const int channels); /** * Return true if the motor channel is off in fixed wing mode. diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 9161025cf3..fbff469f19 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -595,9 +595,6 @@ void VtolAttitudeControl::task_main() parameters_update(); // initialize parameter cache - // make sure we start with idle in mc mode - _vtol_type->set_idle_mc(); - /* wakeup source*/ px4_pollfd_struct_t fds[1] = {}; fds[0].fd = _actuator_inputs_mc; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index a6dffc088e..6c2d50d8dd 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -67,7 +67,9 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _land_detected = _attc->get_land_detected(); _params = _attc->get_params(); - flag_idle_mc = true; + for (auto &pwm_max : _max_mc_pwm_values.values) { + pwm_max = PWM_DEFAULT_MAX; + } } VtolType::~VtolType() @@ -75,71 +77,6 @@ VtolType::~VtolType() } -/** -* Adjust idle speed for mc mode. -*/ -void VtolType::set_idle_mc() -{ - const char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = px4_open(dev, 0); - - if (fd < 0) { - PX4_WARN("can't open %s", dev); - } - - unsigned servo_count; - int ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - unsigned pwm_value = _params->idle_pwm_mc; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (int i = 0; i < _params->vtol_motor_count; i++) { - pwm_values.values[i] = pwm_value; - pwm_values.channel_count++; - } - - ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) { - PX4_WARN("failed setting min values"); - } - - px4_close(fd); - - flag_idle_mc = true; -} - -/** -* Adjust idle speed for fw mode. -*/ -void VtolType::set_idle_fw() -{ - const char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = px4_open(dev, 0); - - if (fd < 0) { - PX4_WARN("can't open %s", dev); - } - - struct pwm_output_values pwm_values; - - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (int i = 0; i < _params->vtol_motor_count; i++) { - - pwm_values.values[i] = PWM_MOTOR_OFF; - pwm_values.channel_count++; - } - - int ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) { - PX4_WARN("failed setting min values"); - } - - px4_close(fd); -} - void VtolType::update_mc_state() { // copy virtual attitude setpoint to real attitude setpoint @@ -263,5 +200,87 @@ void VtolType::check_quadchute_condition() } } } - +} + +bool +VtolType::disable_mc_motors() +{ + const char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = px4_open(dev, 0); + + if (fd < 0) { + PX4_ERR("can't open %s", dev); + return false; + } + + // first save the current max values + struct pwm_output_values max_pwm_values = {}; + + int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&max_pwm_values); + + if (ret == OK) { + _max_mc_pwm_values = max_pwm_values; + + } else { + PX4_ERR("failed getting max values"); + px4_close(fd); + return false; + } + + // now get the disarmed PWM values + pwm_output_values disarmed_pwm_values = {}; + ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&disarmed_pwm_values); + + if (ret == OK) { + + // finally disable by setting the MC motors max to the disarmed value + max_pwm_values.channel_count = _params->vtol_motor_count; + + for (int i = 0; i < _params->vtol_motor_count; i++) { + max_pwm_values.values[i] = disarmed_pwm_values.values[i]; + } + + ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&max_pwm_values); + + if (ret != OK) { + PX4_ERR("failed setting max values"); + } + + } else { + PX4_ERR("failed getting max values"); + } + + px4_close(fd); + + return (ret == PX4_OK); +} + +bool +VtolType::enable_mc_motors() +{ + const char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = px4_open(dev, 0); + + if (fd < 0) { + PX4_ERR("can't open %s", dev); + return false; + } + + struct pwm_output_values pwm_values = {}; + + pwm_values.channel_count = _params->vtol_motor_count; + + for (int i = 0; i < _params->vtol_motor_count; i++) { + pwm_values.values[i] = _max_mc_pwm_values.values[i]; + } + + int ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) { + PX4_ERR("failed setting max values"); + } + + px4_close(fd); + + return (ret == PX4_OK); } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 5d358e277b..9457464237 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -45,6 +45,7 @@ #include #include +#include struct Params { int32_t idle_pwm_mc; // pwm value for idle in mc mode @@ -182,10 +183,17 @@ protected: float _ra_hrate_sp = 0.0f; // rolling average on height rate setpoint for quadchute condition bool _flag_was_in_trans_mode = false; // true if mode has just switched to transition + hrt_abstime _trans_finished_ts = 0; + bool _tecs_running = false; hrt_abstime _tecs_running_ts = 0; + struct pwm_output_values _max_mc_pwm_values {}; + + bool enable_mc_motors(); + bool disable_mc_motors(); + }; #endif