fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-08 19:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

use FW attitude controller in FW but always MC rate controller

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-08-09 10:12:08 +02:00
parent e637e63d21
commit aa87a342e6
3 changed files with 57 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+10
View File
@@ -151,6 +151,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
_att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
// TODO rotate thrust prior to publication for tailsitter?
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
@@ -602,6 +603,15 @@ void FixedwingAttitudeControl::Run()
_rates_sp.timestamp = hrt_absolute_time();
// rotate output of FW attitude controller to body frame
if (_vehicle_status.is_vtol_tailsitter) {
const float tmp = _rates_sp.roll;
_rates_sp.roll = _rates_sp.yaw;
_rates_sp.yaw = -tmp;
_rates_sp.thrust_body[2] = -_rates_sp.thrust_body[0];
_rates_sp.thrust_body[0] = 0.f;
}
_rate_sp_pub.publish(_rates_sp);
} else {
src/modules/mc_att_control/mc_att_control_main.cpp
+6 -1
View File
@@ -303,7 +303,12 @@ MulticopterAttitudeControl::Run()
const bool is_hovering = (_vehicle_type_rotary_wing && !_vtol_in_transition_mode);
bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || _vtol_tailsitter);
const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode);
const bool run_mc_att_c_in_fw_tailsitter = false; // TODO: just for testing
bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition
|| run_mc_att_c_in_fw_tailsitter);
if (run_att_ctrl) {
src/modules/vtol_att_control/tailsitter.cpp
+41 -27
View File
@@ -207,11 +207,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
if (_v_control_mode->flag_control_altitude_enabled) {
_q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp);
const bool run_mc_att_c_in_fw_tailsitter = false; // TODO: just for testing
if (run_mc_att_c_in_fw_tailsitter) {
_q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, yaw_sp);
} else {
_q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, yaw_sp);
_q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp);
}
// attitude during transitions are controlled by mc attitude control so rotate the desired attitude to the
@@ -295,22 +297,36 @@ void Tailsitter::update_fw_state()
_flag_idle_mc = !set_idle_fw();
}
const bool run_mc_att_c_in_fw_tailsitter = false; // TODO: just for testing
// 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));
if (run_mc_att_c_in_fw_tailsitter) {
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
} else {
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
memcpy(_v_att_sp, _fw_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));
// printf("""""""""""""\n");
// printf("roll sp: %f\n", (double)_v_att_sp->roll_body);
// printf("pitch sp: (raw) %f\n", (double)_v_att_sp->pitch_body);
// printf("yaw_sp: %f\n", (double)_v_att_sp->yaw_body);
_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis, M_PI_2_F));
// // rotate setpoint 90° down
// _v_att_sp->roll_body = _v_att_sp->roll_body;
// _v_att_sp->pitch_body = _v_att_sp->pitch_body - M_PI_2_F;
// _v_att_sp->yaw_body = _v_att_sp->yaw_body;
// Quatf q(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
// q.copyTo(_v_att_sp->q_d);
// // 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));
@@ -320,19 +336,17 @@ void Tailsitter::update_fw_state()
// _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();
// 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);
// printf("roll sp: %f\n", (double)_v_att_sp->roll_body);
// printf("pitch sp: (rotated) %f\n", (double)_v_att_sp->pitch_body);
// printf("yaw_sp: %f\n", (double)_v_att_sp->yaw_body);
_q_trans_sp.copyTo(_v_att_sp->q_d);
// _q_trans_sp.copyTo(_v_att_sp->q_d);
}
@@ -382,10 +396,10 @@ void Tailsitter::fill_actuator_outputs()
// FW thrust is allocated on mc_thrust_sp[0] for tailsitter with dynamic control allocation
_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)
// output differential thrust for roll (FW frame) if enabled (to achieve roll contorl in FW we need controller yaw output)
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::ROLL_BIT)) {
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() ;
mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_r.get() ;
_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_r.get() ;
}
// output differential thrust for pitch if enabled
@@ -394,10 +408,10 @@ void Tailsitter::fill_actuator_outputs()
_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)
// output differential thrust for yaw (FW frame) if enabled (to achieve yaw contorl in FW we need controller roll output)
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT)) {
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() ;
mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_y.get() ;
_torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_y.get() ;
}
} else {