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

(WIP) MC-Att-Ctrl: Added 3d thrust set point to the 6-Dof attitude and rate controllers

Browse Source
This commit is contained in:
Azarakhsh Keipour
2020-01-21 18:23:07 -05:00
parent de9942b204
commit 3a321302f0
3 changed files with 16 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp
+2 -3
View File
@@ -336,9 +336,8 @@ MultirotorMixer6dof::mix(float *outputs, unsigned space)
float roll = math::constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
float pitch = math::constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
float yaw = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
float x_thrust = math::constrain(get_control(0, 4), 0.0f, 1.0f);
float y_thrust = math::constrain(get_control(0, 5), 0.0f, 1.0f);
// TODO remove the - sign
float x_thrust = math::constrain(get_control(0, 8), 0.0f, 1.0f);
float y_thrust = math::constrain(get_control(0, 9), 0.0f, 1.0f);
float z_thrust = - math::constrain(get_control(0, 3), 0.0f, 1.0f);
// clean out class variable used to capture saturation
src/modules/mc_rate_control/MulticopterRateControl.cpp
+13 -6
View File
@@ -213,16 +213,18 @@ MulticopterRateControl::Run()
math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
_rates_sp = man_rate_sp.emult(_acro_rate_max);
_thrust_sp = _manual_control_sp.z;
_thrust_body_sp(0) = _manual_control_sp.x;
_thrust_body_sp(1) = _manual_control_sp.y;
_thrust_body_sp(2) = _manual_control_sp.z;
// publish rate setpoint
vehicle_rates_setpoint_s v_rates_sp{};
v_rates_sp.roll = _rates_sp(0);
v_rates_sp.pitch = _rates_sp(1);
v_rates_sp.yaw = _rates_sp(2);
v_rates_sp.thrust_body[0] = 0.0f;
v_rates_sp.thrust_body[1] = 0.0f;
v_rates_sp.thrust_body[2] = -_thrust_sp;
v_rates_sp.thrust_body[0] = _thrust_body_sp(0);
v_rates_sp.thrust_body[1] = _thrust_body_sp(1);
v_rates_sp.thrust_body[2] = _thrust_body_sp(2);
v_rates_sp.timestamp = hrt_absolute_time();
_v_rates_sp_pub.publish(v_rates_sp);
@@ -236,7 +238,9 @@ MulticopterRateControl::Run()
_rates_sp(0) = v_rates_sp.roll;
_rates_sp(1) = v_rates_sp.pitch;
_rates_sp(2) = v_rates_sp.yaw;
_thrust_sp = -v_rates_sp.thrust_body[2];
_thrust_body_sp(0) = v_rates_sp.thrust_body[0];
_thrust_body_sp(1) = v_rates_sp.thrust_body[1];
_thrust_body_sp(2) = v_rates_sp.thrust_body[2];
}
}
@@ -288,8 +292,11 @@ MulticopterRateControl::Run()
actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_body_sp(2)) ? -_thrust_body_sp(2) : 0.0f;
actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear;
actuators.control[actuator_controls_s::INDEX_X_THRUST] = (PX4_ISFINITE(_thrust_body_sp(0))) ? _thrust_body_sp(0) : 0.0f;
actuators.control[actuator_controls_s::INDEX_Y_THRUST] = (PX4_ISFINITE(_thrust_body_sp(1))) ? _thrust_body_sp(1) : 0.0f;
actuators.control[actuator_controls_s::INDEX_Z_THRUST] = (PX4_ISFINITE(_thrust_body_sp(2))) ? _thrust_body_sp(2) : 0.0f;
actuators.timestamp_sample = angular_velocity.timestamp_sample;
// scale effort by battery status if enabled
src/modules/mc_rate_control/MulticopterRateControl.hpp
+1 -1
View File
@@ -129,7 +129,7 @@ private:
matrix::Vector3f _rates_sp; /**< angular rates setpoint */
float _thrust_sp{0.0f}; /**< thrust setpoint */
matrix::Vector3f _thrust_body_sp; /**< 3-D thrust setpoint */
bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */