diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 52c6258c39..d840f6c7e0 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -46,5 +46,6 @@ set PWM_AUX_MAX 2000 set MAV_TYPE 21 param set VT_MOT_COUNT 6 +param set VT_FW_MOT_OFF 23 param set VT_IDLE_PWM_MC 1080 param set VT_TYPE 1 diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 68e4aa9dc6..244b8c1d92 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -106,6 +106,7 @@ uint32 component_id # subsystem / component id, inspired by MAVLink's componen bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter bool is_vtol # True if the system is VTOL capable bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode +bool in_transition_mode bool condition_battery_voltage_valid bool condition_system_in_air_restore # true if we can restore in mid air diff --git a/msg/vtol_vehicle_status.msg b/msg/vtol_vehicle_status.msg index 5b2dc991c9..a1be5d5867 100644 --- a/msg/vtol_vehicle_status.msg +++ b/msg/vtol_vehicle_status.msg @@ -1,4 +1,5 @@ uint64 timestamp # Microseconds since system boot bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode +bool vtol_in_trans_mode bool fw_permanent_stab # In fw mode stabilize attitude even if in manual mode float32 airspeed_tot # Estimated airspeed over control surfaces diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4ac9f939c3..e103de3f61 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -197,6 +197,8 @@ static struct home_position_s _home; static unsigned _last_mission_instance = 0; +struct vtol_vehicle_status_s vtol_status; + /** * The daemon app only briefly exists to start * the background job. The stack size assigned in the @@ -1169,7 +1171,7 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to vtol vehicle status topic */ int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); - struct vtol_vehicle_status_s vtol_status; + //struct vtol_vehicle_status_s vtol_status; memset(&vtol_status, 0, sizeof(vtol_status)); vtol_status.vtol_in_rw_mode = true; //default for vtol is rotary wing @@ -1496,6 +1498,7 @@ int commander_thread_main(int argc, char *argv[]) /* Make sure that this is only adjusted if vehicle really is of type vtol*/ if (is_vtol(&status)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; + status.in_transition_mode = vtol_status.vtol_in_trans_mode; } } @@ -2682,13 +2685,13 @@ set_control_mode() !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_acceleration_force; - control_mode.flag_control_velocity_enabled = !offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position; + control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_position) && !vtol_status.vtol_in_trans_mode; control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_position; - control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position; + control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !vtol_status.vtol_in_trans_mode; control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_position; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index ee78ef3110..c3f83e21ba 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -941,7 +941,7 @@ FixedwingAttitudeControl::task_main() att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ - if (!_vehicle_status.is_rotary_wing) { + if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { if (_attitude_sp_pub != nullptr) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 8d2ebbb6af..213af56c25 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -993,6 +993,11 @@ MulticopterPositionControl::task_main() reset_yaw_sp = true; } + // XXX Temporary: for vtol use we need to reset the yaw setpoint when we are doing a transition + if (_vehicle_status.in_transition_mode) { + reset_yaw_sp = true; + } + //Update previous arming state was_armed = _control_mode.flag_armed; @@ -1459,7 +1464,7 @@ MulticopterPositionControl::task_main() if (!(_control_mode.flag_control_offboard_enabled && !(_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled))) { - if (_att_sp_pub != nullptr && _vehicle_status.is_rotary_wing) { + if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); } else if (_att_sp_pub == nullptr && _vehicle_status.is_rotary_wing){ _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index c38a935ff7..1d9d7b1155 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -46,12 +46,15 @@ Standard::Standard(VtolAttitudeControl *attc) : VtolType(attc), _flag_enable_mc_motors(true), _pusher_throttle(0.0f), - _mc_att_ctl_weight(1.0f), _airspeed_trans_blend_margin(0.0f) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + _params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR"); _params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR"); _params_handles_standard.pusher_trans = param_find("VT_TRANS_THR"); @@ -107,7 +110,9 @@ void Standard::update_vtol_state() if (_vtol_schedule.flight_mode == MC_MODE) { // in mc mode _vtol_schedule.flight_mode = MC_MODE; - _mc_att_ctl_weight = 1.0f; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; } else if (_vtol_schedule.flight_mode == FW_MODE) { // transition to mc mode @@ -118,7 +123,9 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // failsafe back to mc mode _vtol_schedule.flight_mode = MC_MODE; - _mc_att_ctl_weight = 1.0f; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // keep transitioning to mc mode @@ -138,7 +145,9 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == FW_MODE) { // in fw mode _vtol_schedule.flight_mode = FW_MODE; - _mc_att_ctl_weight = 0.0f; + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + _mc_yaw_weight = 0.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode @@ -184,18 +193,28 @@ void Standard::update_transition_state() // do blending of mc and fw controls if a blending airspeed has been provided if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) { - _mc_att_ctl_weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin; + float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin; + _mc_roll_weight = weight; + _mc_pitch_weight = weight; + _mc_yaw_weight = weight; } else { // at low speeds give full weight to mc - _mc_att_ctl_weight = 1.0f; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // continually increase mc attitude control as we transition back to mc mode if (_params_standard.back_trans_dur > 0.0f) { - _mc_att_ctl_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f); + float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f); + _mc_roll_weight = weight; + _mc_pitch_weight = weight; + _mc_yaw_weight = weight; } else { - _mc_att_ctl_weight = 1.0f; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; } // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again @@ -206,7 +225,9 @@ void Standard::update_transition_state() } } - _mc_att_ctl_weight = math::constrain(_mc_att_ctl_weight, 0.0f, 1.0f); + _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); + _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); + _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); } void Standard::update_mc_state() @@ -214,16 +235,6 @@ void Standard::update_mc_state() // do nothing } -void Standard::process_mc_data() -{ - fill_att_control_output(); -} - -void Standard::process_fw_data() -{ - fill_att_control_output(); -} - void Standard::update_fw_state() { // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again @@ -242,27 +253,26 @@ void Standard::update_external_state() * Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine * what proportion of control should be applied to each of the control groups (mc and fw). */ -void Standard::fill_att_control_output() +void Standard::fill_actuator_outputs() { /* multirotor controls */ - _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _mc_att_ctl_weight; // roll - _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _mc_att_ctl_weight; // pitch - _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _mc_att_ctl_weight; // yaw - _actuators_out_0->control[3] = _actuators_mc_in->control[3]; // throttle + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; // roll + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; // yaw + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle /* fixed wing controls */ - const float fw_att_ctl_weight = 1.0f - _mc_att_ctl_weight; - _actuators_out_1->control[0] = -_actuators_fw_in->control[0] * fw_att_ctl_weight; //roll - _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim) * fw_att_ctl_weight; //pitch - _actuators_out_1->control[2] = _actuators_fw_in->control[2] * fw_att_ctl_weight; // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //roll + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); //pitch + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw // set the fixed wing throttle control if (_vtol_schedule.flight_mode == FW_MODE) { // take the throttle value commanded by the fw controller - _actuators_out_1->control[3] = _actuators_fw_in->control[3]; + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; } else { // otherwise we may be ramping up the throttle during the transition to fw mode - _actuators_out_1->control[3] = _pusher_throttle; + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; } } diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index e4e1d92d85..7bce82071a 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -57,9 +57,7 @@ public: void update_vtol_state(); void update_mc_state(); - void process_mc_data(); void update_fw_state(); - void process_fw_data(); void update_transition_state(); void update_external_state(); @@ -95,10 +93,9 @@ private: bool _flag_enable_mc_motors; float _pusher_throttle; - float _mc_att_ctl_weight; // the amount of multicopter attitude control that should be applied in fixed wing mode while transitioning float _airspeed_trans_blend_margin; - void fill_att_control_output(); + void fill_actuator_outputs(); void set_max_mc(unsigned pwm_value); int parameters_update(); diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 358f636561..46fd7b9c3d 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -73,13 +73,6 @@ void Tailsitter::update_mc_state() } } -void Tailsitter::process_mc_data() -{ - // scale pitch control with total airspeed - //scale_mc_output(); - fill_mc_att_control_output(); -} - void Tailsitter::update_fw_state() { if (flag_idle_mc) { @@ -88,11 +81,6 @@ void Tailsitter::update_fw_state() } } -void Tailsitter::process_fw_data() -{ - fill_fw_att_control_output(); -} - void Tailsitter::update_transition_state() { @@ -152,38 +140,41 @@ Tailsitter::scale_mc_output() } /** -* Prepare message to acutators with data from fw attitude controller. +* Write data to actuator output topic. */ -void Tailsitter::fill_fw_att_control_output() +void Tailsitter::fill_actuator_outputs() { - /*For the first test in fw mode, only use engines for thrust!!!*/ - _actuators_out_0->control[0] = 0; - _actuators_out_0->control[1] = 0; - _actuators_out_0->control[2] = 0; - _actuators_out_0->control[3] = _actuators_fw_in->control[3]; - /*controls for the elevons */ - _actuators_out_1->control[0] = -_actuators_fw_in->control[0]; // roll elevon - _actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim; // pitch elevon - // unused now but still logged - _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw - _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle -} + switch(_vtol_mode) { + case ROTARY_WING: + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; -/** -* Prepare message to acutators with data from mc attitude controller. -*/ -void Tailsitter::fill_mc_att_control_output() -{ - _actuators_out_0->control[0] = _actuators_mc_in->control[0]; - _actuators_out_0->control[1] = _actuators_mc_in->control[1]; - _actuators_out_0->control[2] = _actuators_mc_in->control[2]; - _actuators_out_0->control[3] = _actuators_mc_in->control[3]; + if (_params->elevons_mc_lock == 1) { + _actuators_out_1->control[0] = 0; + _actuators_out_1->control[1] = 0; + } else { + // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon + } + break; + case FIXED_WING: + // in fixed wing mode we use engines only for providing thrust, no moments are generated + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - if (_params->elevons_mc_lock == 1) { - _actuators_out_1->control[0] = 0; - _actuators_out_1->control[1] = 0; - } else { - _actuators_out_1->control[0] = _actuators_mc_in->control[2]; //roll elevon - _actuators_out_1->control[1] = _actuators_mc_in->control[1]; //pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle + break; + case TRANSITION: + case EXTERNAL: + // not yet implemented, we are switching brute force at the moment + break; } } diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index e681a9bf8b..388111ace8 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -53,15 +53,12 @@ public: void update_vtol_state(); void update_mc_state(); - void process_mc_data(); void update_fw_state(); - void process_fw_data(); void update_transition_state(); void update_external_state(); private: - void fill_mc_att_control_output(); - void fill_fw_att_control_output(); + void fill_actuator_outputs(); void calc_tot_airspeed(); void scale_mc_output(); diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index e7ebab58e9..06cde02a70 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -41,25 +41,34 @@ #include "tiltrotor.h" #include "vtol_att_control_main.h" -#define ARSP_BLEND_START 8.0f // airspeed at which we start blending mc/fw controls +#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : VtolType(attc), -flag_max_mc(true), +_rear_motors(ENABLED), _tilt_control(0.0f), -_roll_weight_mc(1.0f) +_roll_weight_mc(1.0f), +_yaw_weight_mc(1.0f), +_min_front_trans_dur(0.5f) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + _params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR"); _params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR"); _params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC"); _params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS"); _params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); _params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS"); + _params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND"); _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); - } + _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); + _params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFF"); +} Tiltrotor::~Tiltrotor() { @@ -72,6 +81,11 @@ 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 duration of a front transition */ param_get(_params_handles_tiltrotor.front_trans_dur, &v); _params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f); @@ -96,13 +110,44 @@ Tiltrotor::parameters_update() param_get(_params_handles_tiltrotor.airspeed_trans, &v); _params_tiltrotor.airspeed_trans = v; + /* vtol airspeed at which we start blending mc/fw controls */ + param_get(_params_handles_tiltrotor.airspeed_blend_start, &v); + _params_tiltrotor.airspeed_blend_start = v; + /* vtol lock elevons in multicopter */ param_get(_params_handles_tiltrotor.elevons_mc_lock, &l); _params_tiltrotor.elevons_mc_lock = l; + /* vtol front transition phase 2 duration */ + param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v); + _params_tiltrotor.front_trans_dur_p2 = v; + + /* avoid parameters which will lead to zero division in the transition code */ + _params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur); + + if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) { + _params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f; + } + return OK; } +int 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; + channels = channels / 10; + } + + return channel_bitmap; +} + void Tiltrotor::update_vtol_state() { parameters_update(); @@ -113,53 +158,60 @@ void Tiltrotor::update_vtol_state() * forward completely. For the backtransition the motors simply rotate back. */ - if (_manual_control_sp->aux1 < 0.0f && _vtol_schedule.flight_mode == MC_MODE) { - // mc mode - _vtol_schedule.flight_mode = MC_MODE; - _tilt_control = _params_tiltrotor.tilt_mc; - _roll_weight_mc = 1.0f; - } else if (_manual_control_sp->aux1 < 0.0f && _vtol_schedule.flight_mode == FW_MODE) { - _vtol_schedule.flight_mode = TRANSITION_BACK; - flag_max_mc = true; - _vtol_schedule.transition_start = hrt_absolute_time(); - } else if (_manual_control_sp->aux1 >= 0.0f && _vtol_schedule.flight_mode == MC_MODE) { - // instant of doeing a front-transition - _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; - _vtol_schedule.transition_start = hrt_absolute_time(); - } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1 && _manual_control_sp->aux1 > 0.0f) { - // check if we have reached airspeed to switch to fw mode - if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) { - _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; - flag_max_mc = true; - _vtol_schedule.transition_start = hrt_absolute_time(); - } - } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2 && _manual_control_sp->aux1 > 0.0f) { - if (_tilt_control >= _params_tiltrotor.tilt_fw) { - _vtol_schedule.flight_mode = FW_MODE; - _tilt_control = _params_tiltrotor.tilt_fw; - } - } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1 && _manual_control_sp->aux1 < 0.0f) { - // failsave into mc mode - _vtol_schedule.flight_mode = MC_MODE; - _tilt_control = _params_tiltrotor.tilt_mc; - } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2 && _manual_control_sp->aux1 < 0.0f) { - // failsave into mc mode - _vtol_schedule.flight_mode = MC_MODE; - _tilt_control = _params_tiltrotor.tilt_mc; - } else if (_vtol_schedule.flight_mode == TRANSITION_BACK && _manual_control_sp->aux1 < 0.0f) { - if (_tilt_control <= _params_tiltrotor.tilt_mc) { - _vtol_schedule.flight_mode = MC_MODE; - _tilt_control = _params_tiltrotor.tilt_mc; - flag_max_mc = false; - } - } else if (_vtol_schedule.flight_mode == TRANSITION_BACK && _manual_control_sp->aux1 > 0.0f) { - // failsave into fw mode - _vtol_schedule.flight_mode = FW_MODE; - _tilt_control = _params_tiltrotor.tilt_fw; - } + if (_manual_control_sp->aux1 < 0.0f) { - // tilt rotors if necessary - update_transition_state(); + // plane is in multicopter mode + switch(_vtol_schedule.flight_mode) { + case MC_MODE: + break; + case FW_MODE: + _vtol_schedule.flight_mode = TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + case TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + case TRANSITION_FRONT_P2: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + case TRANSITION_BACK: + if (_tilt_control <= _params_tiltrotor.tilt_mc) { + _vtol_schedule.flight_mode = MC_MODE; + } + break; + } + } else { + + switch(_vtol_schedule.flight_mode) { + case MC_MODE: + // initialise a front transition + _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + case FW_MODE: + break; + case TRANSITION_FRONT_P1: + // check if we have reached airspeed to switch to fw mode + if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) { + _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; + _vtol_schedule.transition_start = hrt_absolute_time(); + } + break; + case TRANSITION_FRONT_P2: + // if the rotors have been tilted completely we switch to fw mode + if (_tilt_control >= _params_tiltrotor.tilt_fw) { + _vtol_schedule.flight_mode = FW_MODE; + _tilt_control = _params_tiltrotor.tilt_fw; + } + break; + case TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = FW_MODE; + break; + } + } // map tiltrotor specific control phases to simple control modes switch(_vtol_schedule.flight_mode) { @@ -179,10 +231,12 @@ void Tiltrotor::update_vtol_state() void Tiltrotor::update_mc_state() { - // adjust max pwm for rear motors to spin up - if (!flag_max_mc) { - set_max_mc(); - flag_max_mc = true; + // make sure motors are not tilted + _tilt_control = _params_tiltrotor.tilt_mc; + + // enable rear motors + if (_rear_motors != ENABLED) { + set_rear_motor_state(ENABLED); } // set idle speed for rotary wing mode @@ -190,27 +244,20 @@ void Tiltrotor::update_mc_state() set_idle_mc(); flag_idle_mc = true; } -} -void Tiltrotor::process_mc_data() -{ - fill_att_control_output(); + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; } void Tiltrotor::update_fw_state() { - /* in fw mode we need the rear motors to stop spinning, in backtransition - * mode we let them spin in idle - */ - if (flag_max_mc) { - if (_vtol_schedule.flight_mode == TRANSITION_BACK) { - set_max_fw(1200); - set_idle_mc(); - } else { - set_max_fw(950); - set_idle_fw(); - } - flag_max_mc = false; + // make sure motors are tilted forward + _tilt_control = _params_tiltrotor.tilt_fw; + + // disable rear motors + if (_rear_motors != DISABLED) { + set_rear_motor_state(DISABLED); } // adjust idle for fixed wing flight @@ -218,47 +265,68 @@ void Tiltrotor::process_mc_data() set_idle_fw(); flag_idle_mc = false; } - } -void Tiltrotor::process_fw_data() -{ - fill_att_control_output(); + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + _mc_yaw_weight = 0.0f; } 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); + } // tilt rotors forward up to certain angle - if (_params_tiltrotor.front_trans_dur <= 0.0f) { - _tilt_control = _params_tiltrotor.tilt_transition; - } else if (_tilt_control <= _params_tiltrotor.tilt_transition) { - _tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * - (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f); + if (_tilt_control <= _params_tiltrotor.tilt_transition ) { + _tilt_control = _params_tiltrotor.tilt_mc + + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f); } // do blending of mc and fw controls - if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START && _params_tiltrotor.airspeed_trans - ARSP_BLEND_START > 0.0f) { - _roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START); + if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) { + _mc_roll_weight = 0.0f; } else { // at low speeds give full weight to mc - _roll_weight_mc = 1.0f; + _mc_roll_weight = 1.0f; } - _roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f); + // disable mc yaw control once the plane has picked up speed + _mc_yaw_weight = 1.0f; + if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { + _mc_yaw_weight = 0.0f; + } } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { - _tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * - (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (0.5f * 1000000.0f); - _roll_weight_mc = 0.0f; + // the plane is ready to go into fixed wing mode, tilt the rotors forward completely + _tilt_control = _params_tiltrotor.tilt_transition + + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + _mc_roll_weight = 0.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { - // tilt rotors forward up to certain angle - float progress = (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f); - if (_tilt_control > _params_tiltrotor.tilt_mc) { - _tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * progress; + if (_rear_motors != IDLE) { + set_rear_motor_state(IDLE); } - _roll_weight_mc = progress; + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } + // tilt rotors back + if (_tilt_control > _params_tiltrotor.tilt_mc) { + _tilt_control = _params_tiltrotor.tilt_fw - + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); + } + + // set zero throttle for backtransition otherwise unwanted moments will be created + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; + + _mc_roll_weight = 0.0f; + } + + _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); + _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); } void Tiltrotor::update_external_state() @@ -266,35 +334,47 @@ void Tiltrotor::update_external_state() } - /** -* Prepare message to acutators with data from the attitude controllers. +/** +* Write data to actuator output topic. */ -void Tiltrotor::fill_att_control_output() +void Tiltrotor::fill_actuator_outputs() { - _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; // roll - _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; // pitch - _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; // yaw + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; - if (_vtol_schedule.flight_mode == FW_MODE) { - _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // fw throttle - } else { - _actuators_out_0->control[3] = _actuators_mc_in->control[3]; // mc throttle - } + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); - _actuators_out_1->control[0] = -_actuators_fw_in->control[0] * (1.0f - _roll_weight_mc); //roll elevon - _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon - _actuators_out_1->control[4] = _tilt_control; // for tilt-rotor control - - // unused now but still logged - _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // fw yaw + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw + _actuators_out_1->control[4] = _tilt_control; } + /** -* Kill rear motors for the FireFLY6 when in fw mode. +* Set state of rear motors. */ -void -Tiltrotor::set_max_fw(unsigned pwm_value) -{ + +void Tiltrotor::set_rear_motor_state(rear_motor_state state) { + 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_LOWEST_MAX; + _rear_motors = DISABLED; + break; + case IDLE: + pwm_value = _params->idle_pwm_mc; + _rear_motors = IDLE; + break; + } + int ret; unsigned servo_count; char *dev = PWM_OUTPUT0_DEVICE_PATH; @@ -307,10 +387,10 @@ Tiltrotor::set_max_fw(unsigned pwm_value) memset(&pwm_values, 0, sizeof(pwm_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { - if (i == 2 || i == 3) { + if (is_motor_off_channel(i)) { pwm_values.values[i] = pwm_value; } else { - pwm_values.values[i] = 2000; + pwm_values.values[i] = PWM_DEFAULT_MAX; } pwm_values.channel_count = _params->vtol_motor_count; } @@ -322,28 +402,6 @@ Tiltrotor::set_max_fw(unsigned pwm_value) close(fd); } -void -Tiltrotor::set_max_mc() -{ - int ret; - unsigned servo_count; - char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); - - if (fd < 0) {err(1, "can't open %s", dev);} - - ret = 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] = 2000; - pwm_values.channel_count = _params->vtol_motor_count; - } - - ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) {errx(ret, "failed setting max values");} - - 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 07f3562027..859731266a 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -52,24 +52,44 @@ public: Tiltrotor(VtolAttitudeControl * _att_controller); ~Tiltrotor(); + /** + * Update vtol state. + */ void update_vtol_state(); + + /** + * Update multicopter state. + */ void update_mc_state(); - void process_mc_data(); + + /** + * Update fixed wing state. + */ void update_fw_state(); - void process_fw_data(); + + /** + * Update transition state. + */ void update_transition_state(); + + /** + * Update external state. + */ void update_external_state(); private: struct { - float front_trans_dur; - float back_trans_dur; - float tilt_mc; - float tilt_transition; - float tilt_fw; - float airspeed_trans; - int elevons_mc_lock; // lock elevons in multicopter mode + float front_trans_dur; /**< duration of first part of front transition */ + float back_trans_dur; /**< duration of back transition */ + float tilt_mc; /**< actuator value corresponding to mc tilt */ + float tilt_transition; /**< actuator value corresponding to transition tilt (e.g 45 degrees) */ + float tilt_fw; /**< actuator value corresponding to fw tilt */ + float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */ + float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ + int elevons_mc_lock; /**< lock elevons in multicopter mode */ + float front_trans_dur_p2; + int fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */ } _params_tiltrotor; struct { @@ -79,30 +99,65 @@ private: param_t tilt_transition; param_t tilt_fw; param_t airspeed_trans; + param_t airspeed_blend_start; param_t elevons_mc_lock; + param_t front_trans_dur_p2; + param_t fw_motors_off; } _params_handles_tiltrotor; enum vtol_mode { - MC_MODE = 0, - TRANSITION_FRONT_P1, - TRANSITION_FRONT_P2, - TRANSITION_BACK, - FW_MODE + MC_MODE = 0, /**< vtol is in multicopter mode */ + TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */ + TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */ + TRANSITION_BACK, /**< vtol is in back transition mode */ + FW_MODE /**< vtol is in fixed wing mode */ }; + /** + * Specific to tiltrotor with vertical aligned rear engine/s. + * 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 + } _rear_motors; + struct { - vtol_mode flight_mode; // indicates in which mode the vehicle is in - hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) + vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ + hrt_abstime transition_start; /**< absoulte time at which front transition started */ }_vtol_schedule; - bool flag_max_mc; - float _tilt_control; - float _roll_weight_mc; + float _tilt_control; /**< actuator value for the tilt servo */ + float _roll_weight_mc; /**< multicopter desired roll moment weight */ + float _yaw_weight_mc; /**< multicopter desired yaw moment weight */ - void fill_att_control_output(); - void set_max_mc(); - void set_max_fw(unsigned pwm_value); + const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ + /** + * Return a bitmap of channels that should be turned off in fixed wing mode. + */ + int 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); + + /** + * Write control values to actuator output topics. + */ + void fill_actuator_outputs(); + + /** + * Adjust the state of the rear motors. In fw mode they shouldn't spin. + */ + void set_rear_motor_state(rear_motor_state state); + + /** + * Update parameters. + */ int parameters_update(); }; diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index d4e925f072..d1b5b31081 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -72,3 +72,25 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); + +/** + * Duration of front transition phase 2 + * + * Time in seconds it should take for the rotors to rotate forward completely from the point + * when the plane has picked up enough airspeed and is ready to go into fixed wind mode. + * + * @min 0.1 + * @max 2 + * @group VTOL Attitude Control + */ +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 123456 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_MOT_OFF, 0); 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 92a591de51..6f4405d7c8 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -43,7 +43,6 @@ * @author Thomas Gubler * */ - #include "vtol_att_control_main.h" namespace VTOL_att_control @@ -492,34 +491,34 @@ void VtolAttitudeControl::task_main() if (_vtol_type->get_mode() == ROTARY_WING) { // vehicle is in rotary wing mode _vtol_vehicle_status.vtol_in_rw_mode = true; - - _vtol_type->update_mc_state(); + _vtol_vehicle_status.vtol_in_trans_mode = false; // got data from mc attitude controller if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); - _vtol_type->process_mc_data(); + _vtol_type->update_mc_state(); fill_mc_att_rates_sp(); } } else if (_vtol_type->get_mode() == FIXED_WING) { // vehicle is in fw mode _vtol_vehicle_status.vtol_in_rw_mode = false; - - _vtol_type->update_fw_state(); + _vtol_vehicle_status.vtol_in_trans_mode = false; // got data from fw attitude controller if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); vehicle_manual_poll(); - _vtol_type->process_fw_data(); + _vtol_type->update_fw_state(); fill_fw_att_rates_sp(); } } else if (_vtol_type->get_mode() == TRANSITION) { // vehicle is doing a transition + _vtol_vehicle_status.vtol_in_trans_mode = true; + bool got_new_data = false; if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); @@ -534,8 +533,6 @@ void VtolAttitudeControl::task_main() // update transition state if got any new data if (got_new_data) { _vtol_type->update_transition_state(); - - _vtol_type->process_mc_data(); fill_mc_att_rates_sp(); } @@ -544,6 +541,7 @@ void VtolAttitudeControl::task_main() _vtol_type->update_external_state(); } + _vtol_type->fill_actuator_outputs(); /* Only publish if the proper mode(s) are enabled */ if(_v_control_mode.flag_control_attitude_enabled || diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index a7fdbdde1a..a797201e01 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -41,6 +41,8 @@ #ifndef VTOL_TYPE_H #define VTOL_TYPE_H +#include + struct Params { int idle_pwm_mc; // pwm value for idle in mc mode int vtol_motor_count; // number of motors @@ -75,11 +77,10 @@ public: virtual void update_vtol_state() = 0; virtual void update_mc_state() = 0; - virtual void process_mc_data() = 0; virtual void update_fw_state() = 0; - virtual void process_fw_data() = 0; virtual void update_transition_state() = 0; virtual void update_external_state() = 0; + virtual void fill_actuator_outputs() = 0; void set_idle_mc(); void set_idle_fw(); @@ -111,6 +112,10 @@ protected: bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" + float _mc_roll_weight; // weight for multicopter attitude controller roll output + float _mc_pitch_weight; // weight for multicopter attitude controller pitch output + float _mc_yaw_weight; // weight for multicopter attitude controller yaw output + }; #endif