diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 88eb51aa51..fb65472db9 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -303,10 +303,7 @@ MulticopterAttitudeControl::Run() const bool is_hovering = (_vehicle_type_rotary_wing && !_vtol_in_transition_mode); - // vehicle is a tailsitter in transition mode - const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode); - - bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition); + bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || _vtol_tailsitter); if (run_att_ctrl) { diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index e639b9d153..98383c97aa 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -209,8 +209,9 @@ MulticopterRateControl::Run() // run the rate controller if (_vehicle_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) { - // reset integral if disarmed - if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + // reset integral if disarmed or we do not run the controller (VTOLs not in hover except tailsitter) + if (!_vehicle_control_mode.flag_armed || (_vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.is_vtol_tailsitter)) { _rate_control.resetIntegral(); } diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 2f37ceac44..7f0e188d49 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -206,7 +206,13 @@ void Tailsitter::update_transition_state() // the intial attitude setpoint for a backtransition is a combination of the current fw pitch setpoint, // the yaw setpoint and zero roll since we want wings level transition - _q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp); + + if (_v_control_mode->flag_control_altitude_enabled) { + _q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp); + + } else { + _q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, yaw_sp); + } // attitude during transitions are controlled by mc attitude control so rotate the desired attitude to the // multirotor frame @@ -285,7 +291,48 @@ void Tailsitter::waiting_on_tecs() void Tailsitter::update_fw_state() { - VtolType::update_fw_state(); + if (_flag_idle_mc) { + _flag_idle_mc = !set_idle_fw(); + } + + // copy virtual attitude setpoint to real attitude setpoint + if (_v_control_mode->flag_control_altitude_enabled) { + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + + } else { + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + } + + check_quadchute_condition(); + + // calculate rotation axis for transition. + _q_trans_start = Quatf(_v_att->q); + Vector3f z = -_q_trans_start.dcm_z(); + _trans_rot_axis = z.cross(Vector3f(0, 0, -1)); + + _q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis, M_PI_2_F)); + + // // as heading setpoint we choose the heading given by the direction the vehicle points + // float yaw_sp = atan2f(z(1), z(0)); + + // // the intial attitude setpoint for a backtransition is a combination of the current fw pitch setpoint, + // // the yaw setpoint and zero roll since we want wings level transition + // _q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp); + + + const Eulerf euler_sp(_q_trans_sp); + _v_att_sp->roll_body = euler_sp.phi(); + _v_att_sp->pitch_body = euler_sp.theta(); + _v_att_sp->yaw_body = euler_sp.psi(); + + + // printf("""""""""""""\n"); + // printf("phi: %f\n", (double)euler_sp.phi()); + // printf("theta: %f\n", (double)euler_sp.theta()); + // printf("psi: %f\n", (double)euler_sp.psi()); + // printf("xxxxx: %\n", (double)xxxxx); + + _q_trans_sp.copyTo(_v_att_sp->q_d); } @@ -330,27 +377,27 @@ void Tailsitter::fill_actuator_outputs() mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW]; if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { - mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE]; + mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE]; // FW thrust is allocated on mc_thrust_sp[0] for tailsitter with dynamic control allocation - _thrust_setpoint_0->xyz[2] = -fw_in[actuator_controls_s::INDEX_THROTTLE]; + _thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE]; // output differential thrust for roll if enabled (to achieve roll contorl in FW we need controller yaw output) if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::ROLL_BIT)) { - mc_out[actuator_controls_s::INDEX_YAW] = -fw_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_r.get() ; - _torque_setpoint_0->xyz[2] = -fw_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_r.get() ; + mc_out[actuator_controls_s::INDEX_YAW] = -mc_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_r.get() ; + _torque_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_r.get() ; } // output differential thrust for pitch if enabled if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::PITCH_BIT)) { - mc_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get() ; - _torque_setpoint_0->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get() ; + mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get() ; + _torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get() ; } // output differential thrust for yaw if enabled (to achieve yaw contorl in FW we need controller roll output) if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::YAW_BIT)) { - mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ; - _torque_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ; + mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ; + _torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ; } } else { 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 9acdfc29b6..4601b8e892 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -364,7 +364,7 @@ VtolAttitudeControl::Run() // vehicle is in fw mode _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; - if (fw_att_sp_updated) { + if (mc_att_sp_updated || fw_att_sp_updated) { _vtol_type->update_fw_state(); _vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp); }