From 744f80621493d42bb834ad0fd238e070afe7e194 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 15 Mar 2018 15:51:36 +0100 Subject: [PATCH] vtol_att_control: merge methods to disable a selection of motors - moved methods used to modify channel maximum pwm value to VtolType class so that the can be shared by the differnt vtol types Signed-off-by: Roman --- src/modules/vtol_att_control/standard.cpp | 17 +- src/modules/vtol_att_control/standard.h | 3 - src/modules/vtol_att_control/tailsitter.cpp | 6 +- src/modules/vtol_att_control/tiltrotor.cpp | 121 ++--------- src/modules/vtol_att_control/tiltrotor.h | 24 +-- .../vtol_att_control/tiltrotor_params.c | 11 - .../vtol_att_control_main.cpp | 12 ++ .../vtol_att_control/vtol_att_control_main.h | 1 + .../vtol_att_control_params.c | 11 + src/modules/vtol_att_control/vtol_type.cpp | 199 ++++++++++++++---- src/modules/vtol_att_control/vtol_type.h | 35 ++- 11 files changed, 236 insertions(+), 204 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 53f4d0f957..d95e38c1ad 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -135,7 +135,6 @@ void Standard::update_vtol_state() if (_vtol_vehicle_status->vtol_transition_failsafe == true) { // Failsafe event, engage mc motors immediately _vtol_schedule.flight_mode = MC_MODE; - _flag_enable_mc_motors = true; _pusher_throttle = 0.0f; _reverse_output = 0.0f; @@ -143,7 +142,6 @@ void Standard::update_vtol_state() } else { // Regular backtransition _vtol_schedule.flight_mode = TRANSITION_TO_MC; - _flag_enable_mc_motors = true; _vtol_schedule.transition_start = hrt_absolute_time(); _reverse_output = _params_standard.reverse_output; @@ -194,8 +192,7 @@ void Standard::update_vtol_state() can_transition_on_ground()) { _vtol_schedule.flight_mode = FW_MODE; - // we can turn off the multirotor motors now - _flag_enable_mc_motors = false; + // don't set pusher throttle here as it's being ramped up elsewhere _trans_finished_ts = hrt_absolute_time(); } @@ -300,8 +297,8 @@ void Standard::update_transition_state() } // in back transition we need to start the MC motors again - if (_flag_enable_mc_motors) { - _flag_enable_mc_motors = !enable_mc_motors(); + if (_motor_state != ENABLED) { + _motor_state = set_motor_state(_motor_state, ENABLED); } } @@ -318,8 +315,8 @@ void Standard::update_mc_state() VtolType::update_mc_state(); // enable MC motors here in case we transitioned directly to MC mode - if (_flag_enable_mc_motors) { - _flag_enable_mc_motors = !enable_mc_motors(); + if (_motor_state != ENABLED) { + _motor_state = set_motor_state(_motor_state, ENABLED); } // if the thrust scale param is zero or the drone is on manual mode, @@ -399,8 +396,8 @@ void Standard::update_fw_state() VtolType::update_fw_state(); // stop MC motors in FW mode - if (!_flag_enable_mc_motors) { - _flag_enable_mc_motors = disable_mc_motors(); + if (_motor_state != DISABLED) { + _motor_state = VtolType::set_motor_state(_motor_state, DISABLED); } } diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index ad01f90ee7..30ffdfc1ef 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -98,9 +98,6 @@ private: hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) } _vtol_schedule; - // 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; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index d6784214e4..40e5eeab82 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -214,7 +214,7 @@ void Tailsitter::update_transition_state() } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { if (!flag_idle_mc) { - flag_idle_mc = enable_mc_motors(); + flag_idle_mc = set_idle_mc(); } // create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value @@ -260,7 +260,7 @@ void Tailsitter::update_mc_state() // set idle speed for rotary wing mode if (!flag_idle_mc) { - flag_idle_mc = enable_mc_motors(); + flag_idle_mc = set_idle_mc(); } } @@ -269,7 +269,7 @@ void Tailsitter::update_fw_state() VtolType::update_fw_state(); if (flag_idle_mc) { - flag_idle_mc = !disable_mc_motors(); + flag_idle_mc = !set_idle_fw(); } } diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 8a81eea341..8ac14b63c5 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -46,7 +46,6 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : VtolType(attc), - _rear_motors(ENABLED), _tilt_control(0.0f) { _vtol_schedule.flight_mode = MC_MODE; @@ -62,7 +61,6 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS"); _params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); - _params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFFID"); _params_handles_tiltrotor.diff_thrust = param_find("VT_FW_DIFTHR_EN"); _params_handles_tiltrotor.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC"); } @@ -76,11 +74,6 @@ void Tiltrotor::parameters_update() { float v; - int l; - - /* motors that must be turned off when in fixed wing mode */ - param_get(_params_handles_tiltrotor.fw_motors_off, &l); - _params_tiltrotor.fw_motors_off = get_motor_off_channels(l); /* vtol tilt mechanism position in mc mode */ param_get(_params_handles_tiltrotor.tilt_mc, &v); @@ -104,26 +97,6 @@ Tiltrotor::parameters_update() _params_tiltrotor.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f); } -uint32_t Tiltrotor::get_motor_off_channels(int channels) -{ - int channel_bitmap = 0; - - int channel; - - for (int i = 0; i < _params->vtol_motor_count; ++i) { - channel = channels % 10; - - if (channel == 0) { - break; - } - - channel_bitmap |= 1 << (channel - 1); - channels = channels / 10; - } - - return channel_bitmap; -} - void Tiltrotor::update_vtol_state() { @@ -245,13 +218,13 @@ void Tiltrotor::update_mc_state() _tilt_control = _params_tiltrotor.tilt_mc; // enable rear motors - if (_rear_motors != ENABLED) { - set_rear_motor_state(ENABLED); + if (_motor_state != ENABLED) { + _motor_state = set_motor_state(_motor_state, ENABLED); } // set idle speed for rotary wing mode if (!flag_idle_mc) { - flag_idle_mc = enable_mc_motors(); + flag_idle_mc = set_idle_mc(); } } @@ -263,13 +236,13 @@ void Tiltrotor::update_fw_state() _tilt_control = _params_tiltrotor.tilt_fw; // disable rear motors - if (_rear_motors != DISABLED) { - set_rear_motor_state(DISABLED); + if (_motor_state != DISABLED) { + _motor_state = set_motor_state(_motor_state, DISABLED); } // adjust idle for fixed wing flight if (flag_idle_mc) { - flag_idle_mc = !disable_mc_motors(); + flag_idle_mc = !set_idle_fw(); } } @@ -286,8 +259,8 @@ void Tiltrotor::update_transition_state() if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { // for the first part of the transition the rear rotors are enabled - if (_rear_motors != ENABLED) { - set_rear_motor_state(ENABLED); + if (_motor_state != ENABLED) { + _motor_state = set_motor_state(_motor_state, ENABLED); } // tilt rotors forward up to certain angle @@ -335,17 +308,21 @@ void Tiltrotor::update_transition_state() int rear_value = (1.0f - time_since_trans_start / _params_tiltrotor.front_trans_dur_p2) * (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN; - set_rear_motor_state(VALUE, rear_value); + + _motor_state = set_motor_state(_motor_state, VALUE, rear_value); + _thrust_transition = _mc_virtual_att_sp->thrust; } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { - if (_rear_motors != IDLE) { - set_rear_motor_state(IDLE); + if (_motor_state != ENABLED) { + _motor_state = set_motor_state(_motor_state, ENABLED); } + + // set idle speed for rotary wing mode if (!flag_idle_mc) { - flag_idle_mc = enable_mc_motors(); + flag_idle_mc = set_idle_mc(); } // tilt rotors back @@ -411,69 +388,3 @@ void Tiltrotor::fill_actuator_outputs() _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw _actuators_out_1->control[4] = _tilt_control; } - - -/** -* Set state of rear motors. -*/ - -void Tiltrotor::set_rear_motor_state(rear_motor_state state, int value) -{ - int pwm_value = PWM_DEFAULT_MAX; - - // map desired rear rotor state to max allowed pwm signal - switch (state) { - case ENABLED: - pwm_value = PWM_DEFAULT_MAX; - _rear_motors = ENABLED; - break; - - case DISABLED: - pwm_value = PWM_MOTOR_OFF; - _rear_motors = DISABLED; - break; - - case IDLE: - pwm_value = _params->idle_pwm_mc; - _rear_motors = IDLE; - break; - - case VALUE: - pwm_value = value; - _rear_motors = VALUE; - break; - } - - 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_max_values = {}; - - for (int i = 0; i < _params->vtol_motor_count; i++) { - if (is_motor_off_channel(i)) { - pwm_max_values.values[i] = pwm_value; - - } else { - pwm_max_values.values[i] = PWM_DEFAULT_MAX; - } - - pwm_max_values.channel_count = _params->vtol_motor_count; - } - - int ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values); - - if (ret != OK) { - PX4_ERR("failed setting max values"); - } - - px4_close(fd); -} - -bool Tiltrotor::is_motor_off_channel(const int channel) -{ - return (_params_tiltrotor.fw_motors_off >> channel) & 1; -} diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 25f5e16955..53a8f27659 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -66,7 +66,6 @@ private: float tilt_transition; /**< actuator value corresponding to transition tilt (e.g 45 degrees) */ float tilt_fw; /**< actuator value corresponding to fw tilt */ float front_trans_dur_p2; - int32_t fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */ int32_t diff_thrust; float diff_thrust_scale; } _params_tiltrotor; @@ -76,7 +75,6 @@ private: param_t tilt_transition; param_t tilt_fw; param_t front_trans_dur_p2; - param_t fw_motors_off; param_t diff_thrust; param_t diff_thrust_scale; } _params_handles_tiltrotor; @@ -94,12 +92,7 @@ private: * These engines need to be shut down in fw mode. During the back-transition * they need to idle otherwise they need too much time to spin up for mc mode. */ - enum rear_motor_state { - ENABLED = 0, - DISABLED, - IDLE, - VALUE - } _rear_motors; + struct { vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ @@ -108,21 +101,6 @@ private: float _tilt_control; /**< actuator value for the tilt servo */ - /** - * Return a bitmap of channels that should be turned off in fixed wing mode. - */ - uint32_t get_motor_off_channels(const int channels); - - /** - * Return true if the motor channel is off in fixed wing mode. - */ - bool is_motor_off_channel(const int channel); - - /** - * Adjust the state of the rear motors. In fw mode they shouldn't spin. - */ - void set_rear_motor_state(rear_motor_state state, int value = 0); - /** * Update parameters. */ diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index faef49d031..d3cb6df264 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -86,17 +86,6 @@ PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); */ PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); -/** - * The channel number of motors that must be turned off in fixed wing mode. - * - * @min 0 - * @max 12345678 - * @increment 1 - * @decimal 0 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0); - /** * Differential thrust in forwards flight. * 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 fbff469f19..2a39d45a9d 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -85,6 +85,7 @@ VtolAttitudeControl::VtolAttitudeControl() _params_handles.airspeed_mode = param_find("FW_ARSP_MODE"); _params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT"); _params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE"); + _params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID"); _params_handles.wv_takeoff = param_find("VT_WV_TKO_EN"); @@ -504,6 +505,15 @@ VtolAttitudeControl::parameters_update() _params.airspeed_disabled = l != 0; param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout); param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise); + param_get(_params_handles.fw_motors_off, &_params.fw_motors_off); + + // standard vtol always needs to turn all mc motors off when going into fixed wing mode + // normally the parameter fw_motors_off can be used to specify this, however, since historically standard vtol code + // did not use the interface of the VtolType class to disable motors we will have users flying around with a wrong + // parameter value. Therefore, explicitly set it here such that all motors will be disabled as expected. + if (_params.vtol_type == vtol_type::STANDARD) { + _params.fw_motors_off = 0xff; + } // make sure parameters are feasible, require at least 1 m/s difference between transition and blend airspeed _params.airspeed_blend = math::min(_params.airspeed_blend, _params.transition_airspeed - 1.0f); @@ -595,6 +605,8 @@ void VtolAttitudeControl::task_main() parameters_update(); // initialize parameter cache + _task_should_exit = !_vtol_type->init(); + /* wakeup source*/ px4_pollfd_struct_t fds[1] = {}; fds[0].fd = _actuator_inputs_mc; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 3f353d1d0a..61409be88c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -200,6 +200,7 @@ private: param_t airspeed_mode; param_t front_trans_timeout; param_t mpc_xy_cruise; + param_t fw_motors_off; } _params_handles{}; /* for multicopters it is usual to have a non-zero idle speed of the engines diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 96316eff53..5124fc94c0 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -316,3 +316,14 @@ PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0); * @group Mission */ PARAM_DEFINE_INT32(VT_WV_LND_EN, 0); + +/** + * The channel number of motors that must be turned off in fixed wing mode. + * + * @min 0 + * @max 12345678 + * @increment 1 + * @decimal 0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 6c2d50d8dd..fd5c214f26 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -70,6 +70,10 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : for (auto &pwm_max : _max_mc_pwm_values.values) { pwm_max = PWM_DEFAULT_MAX; } + + for (auto &pwm_disarmed : _disarmed_pwm_values.values) { + pwm_disarmed = PWM_MOTOR_OFF; + } } VtolType::~VtolType() @@ -77,6 +81,38 @@ VtolType::~VtolType() } +bool VtolType::init() +{ + 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; + } + + int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&_max_mc_pwm_values); + + + if (ret != PX4_OK) { + PX4_ERR("failed getting max values"); + px4_close(fd); + return false; + } + + + ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&_disarmed_pwm_values); + + if (ret != PX4_OK) { + PX4_ERR("failed getting disarmed values"); + px4_close(fd); + return false; + } + + return true; + +} + void VtolType::update_mc_state() { // copy virtual attitude setpoint to real attitude setpoint @@ -202,85 +238,162 @@ void VtolType::check_quadchute_condition() } } -bool -VtolType::disable_mc_motors() +/** +* Adjust idle speed for mc mode. +*/ +bool VtolType::set_idle_mc() { const char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); if (fd < 0) { - PX4_ERR("can't open %s", dev); + PX4_WARN("can't open %s", dev); return false; } - // first save the current max values - struct pwm_output_values max_pwm_values = {}; + 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)); - 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; + for (int i = 0; i < _params->vtol_motor_count; i++) { + pwm_values.values[i] = pwm_value; + pwm_values.channel_count++; } - // 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); + ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&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"); + if (ret != OK) { + PX4_WARN("failed setting min values"); } px4_close(fd); - return (ret == PX4_OK); + return ret == PX4_OK; } -bool -VtolType::enable_mc_motors() +/** +* Adjust idle speed for fw mode. +*/ +bool VtolType::set_idle_fw() { const char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); if (fd < 0) { - PX4_ERR("can't open %s", dev); + PX4_WARN("can't open %s", dev); return false; } - struct pwm_output_values pwm_values = {}; + 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); + + return ret == PX4_OK; +} + +/** +* Set state of selected motors. +* See _params->fw_motors_off for selection of motors. +*/ + +motor_state VtolType::set_motor_state(const motor_state current_state, const motor_state next_state, const int value) +{ + struct pwm_output_values pwm_values = {}; pwm_values.channel_count = _params->vtol_motor_count; + // per default all motors are running 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); + switch (next_state) { + case ENABLED: + break; - if (ret != OK) { - PX4_ERR("failed setting max values"); + case DISABLED: + for (int i = 0; i < _params->vtol_motor_count; i++) { + if (is_motor_off_channel(i)) { + pwm_values.values[i] = _disarmed_pwm_values.values[i]; + } + } + + break; + + case IDLE: + + for (int i = 0; i < _params->vtol_motor_count; i++) { + if (is_motor_off_channel(i)) { + pwm_values.values[i] = _params->idle_pwm_mc; + } + } + + break; + + case VALUE: + for (int i = 0; i < _params->vtol_motor_count; i++) { + if (is_motor_off_channel(i)) { + pwm_values.values[i] = value; + } + } + + break; } + const char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = px4_open(dev, 0); + + if (fd < 0) { + PX4_WARN("can't open %s", dev); + return current_state; + } + + + int ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + px4_close(fd); - return (ret == PX4_OK); + if (ret != OK) { + PX4_ERR("failed setting max values"); + return current_state; + } + + return next_state; +} + +bool VtolType::is_motor_off_channel(const int channel) +{ + int channel_bitmap = 0; + + int tmp; + int channels = _params->fw_motors_off; + + for (int i = 0; i < _params->vtol_motor_count; ++i) { + tmp = channels % 10; + + if (tmp == 0) { + break; + } + + channel_bitmap |= 1 << (tmp - 1); + channels = channels / 10; + } + + return (channel_bitmap >> channel) & 1; + } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 9457464237..c5e192e56d 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -70,6 +70,7 @@ struct Params { bool airspeed_disabled; float front_trans_timeout; float mpc_xy_cruise; + int32_t fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */ }; // Has to match 1:1 msg/vtol_vehicle_status.msg @@ -86,6 +87,17 @@ enum vtol_type { STANDARD }; +// these are states that can be applied to a selection of multirotor motors. +// e.g. if we need to shut off some motors after transitioning to fixed wing mode +// we can individually disable them while others might still need to be enabled to produce thrust. +// we can select the target motors via VT_FW_MOT_OFFID +enum motor_state { + ENABLED = 0, // motor max pwm will be set to the standard max pwm value + DISABLED, // motor max pwm will be set to a value that shuts the motor off + IDLE, // motor max pwm will be set to VT_IDLE_PWM_MC + VALUE // motor max pwm will be set to a specific value provided, see set_motor_state() +}; + class VtolAttitudeControl; class VtolType @@ -98,6 +110,11 @@ public: virtual ~VtolType(); + /** + * Initialise. + */ + bool init(); + /** * Update vtol state. */ @@ -139,9 +156,6 @@ public: */ bool can_transition_on_ground(); - void set_idle_mc(); - void set_idle_fw(); - mode get_mode() {return _vtol_mode;} virtual void parameters_update() = 0; @@ -189,10 +203,19 @@ protected: bool _tecs_running = false; hrt_abstime _tecs_running_ts = 0; - struct pwm_output_values _max_mc_pwm_values {}; + motor_state _motor_state = motor_state::DISABLED; - bool enable_mc_motors(); - bool disable_mc_motors(); + bool set_idle_mc(); + bool set_idle_fw(); + + motor_state set_motor_state(const motor_state current_state, const motor_state next_state, const int value = 0); + +private: + + struct pwm_output_values _max_mc_pwm_values {}; + struct pwm_output_values _disarmed_pwm_values {}; + + bool is_motor_off_channel(const int channel); };