fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 01:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Omni Att-Ctrl: Manual mode works smoother now and based on the MC_OMNI_MODE parameter

Browse Source 
- Some observations:
	- It's still not as smooth as expected (or maybe I'm not a good pilot)
	- Acceleration on the body X axis feels lower than the body Y axis (no idea why)
- Flight tested in Gazebo sim in both Manual and Autonomous modes
This commit is contained in:
Azarakhsh Keipour
2019-12-28 17:51:29 -05:00
parent 8fd875f8c7
commit 3ae19f254c
2 changed files with 33 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control.hpp
+3 -1
View File
@@ -157,7 +157,9 @@ 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
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,
(ParamInt<px4::params::MC_OMNI_MODE>) _param_mc_omni_mode
)
bool _is_tailsitter{false};
src/modules/mc_att_control/mc_att_control_main.cpp
+30 -31
View File
@@ -218,43 +218,42 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
///////// Added for Omni ////////////////
// set the euler angles, for logging only, must not be used for control
attitude_setpoint.roll_body = 0;
attitude_setpoint.pitch_body = 0;
/* modify roll/pitch if we're in omni-directional mode */
if (_param_mc_omni_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;
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;
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();
// 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;
if (h_thrust_norm > 1.0F) {
h_thrust /= h_thrust_norm;
}
const float max_xy_thrust_ratio = 0.3;
h_thrust *= max_xy_thrust_ratio;
// 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));
}
const float max_xy_thrust_ratio = 0.3;
h_thrust *= max_xy_thrust_ratio;
// Check if the total thrust has exceeded the maximum
Vector3f total_thrust = Vector3f(h_thrust(0), h_thrust(1), z_thrust);
float total_thrust_norm = total_thrust.norm();
if (total_thrust_norm > 1.0F) {
total_thrust /= total_thrust_norm;
}
attitude_setpoint.thrust_body[0] = throttle_curve(total_thrust(0));
attitude_setpoint.thrust_body[1] = throttle_curve(total_thrust(1));
attitude_setpoint.thrust_body[2] = -throttle_curve(total_thrust(2));
///////// End of added for omni /////////////
attitude_setpoint.timestamp = hrt_absolute_time();
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);