fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 00:14:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Omni Att Ctrl: Manual mode no longer uses the attitude mode and acts like an underactuated UAV

Browse Source
This commit is contained in:
Azarakhsh Keipour 2020-09-29 14:01:24 -04:00
parent 96a496b07d
commit f3cf2d309e
2 changed files with 1 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/mc_att_control/mc_att_control.hpp
View File

@ -157,11 +157,7 @@ private:
_param_mpc_thr_hover, /**< throttle at which vehicle is at hover equilibrium */
(ParamInt<px4::params::MPC_THR_CURVE>) _param_mpc_thr_curve, /**< throttle curve behavior */
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,
/* Omnidirectional vehicle params */
(ParamInt<px4::params::OMNI_ATT_MODE>) _param_omni_att_mode,
(ParamFloat<px4::params::OMNI_DFC_MAX_THR>) _param_omni_dfc_max_thr
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode
)
bool _is_tailsitter{false};

36
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -217,42 +217,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
attitude_setpoint.q_d_valid = true;
attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
/* modify roll/pitch if we're in omni-directional mode */
if (_param_omni_att_mode.get() == 2) {
// set the euler angles, for logging only, must not be used for control
attitude_setpoint.roll_body = 0;
attitude_setpoint.pitch_body = 0;
attitude_setpoint.yaw_body = _man_yaw_sp;
Quatf q_sp_omni = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
q_sp_omni.copyTo(attitude_setpoint.q_d);
attitude_setpoint.q_d_valid = true;
float x_thrust = _manual_control_sp.x;
float y_thrust = _manual_control_sp.y;
float z_thrust = _manual_control_sp.z;
// Check if the total horizontal thrust has exceeded the maximum
Vector2f h_thrust = Vector2f(x_thrust, y_thrust);
float h_thrust_norm = h_thrust.norm();
if (h_thrust_norm > 1.0F) {
h_thrust /= h_thrust_norm;
}
h_thrust *= _param_omni_dfc_max_thr.get();
// Check if the total thrust has exceeded the maximum
Vector3f total_thrust = Vector3f(h_thrust(0), h_thrust(1), z_thrust);
total_thrust = total_thrust / total_thrust.norm() * z_thrust;
attitude_setpoint.thrust_body[0] = throttle_curve(math::abs_t(total_thrust(0))) * math::sign(total_thrust(0));
attitude_setpoint.thrust_body[1] = throttle_curve(math::abs_t(total_thrust(1))) * math::sign(total_thrust(1));
attitude_setpoint.thrust_body[2] = -throttle_curve(total_thrust(2));
}
attitude_setpoint.timestamp = hrt_absolute_time();
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);