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

Omni Att-Ctrl: Added attitude controller for omni vehicles 'Manual' mode

Browse Source 
- Still the control feels weird and need more refining
This commit is contained in:
Azarakhsh Keipour 2019-12-23 19:41:20 -05:00
parent d6c27c6f37
commit 252284d89b
1 changed files with 38 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -217,6 +217,44 @@ 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);
///////// 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;
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;
}
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);