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

Bug Fix(es): 3D-thrust ranges fixed to -1..1 and manual z maps to negative z-thrust now

Browse Source 
- The range for 3D-thrust in the 6-DOF multirotor mixer is changed to -1 to 1 now (fixed from 0 to 1)
- The Z thrust in the 6-DOF multirotor mixer is mapped to the Z-thrust command now (fixed from thrust command)
- The manual Z command in the rate controller maps to the negative Z-thrust (fixed from positive Z)
- The variable _thrust_body_sp in the rate controller renamed to _thrust_sp to be compatible with the older variable removed in the last commit
- The code tested in TakeOff, Manual, Hold, Position and Land modes on both tilted hex and iris

Accepting and working with 3D thrust commands now
This commit is contained in:
Azarakhsh Keipour 2019-09-30 18:25:11 -04:00
parent 3a321302f0
commit 400f8ea4be
3 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp
View File

@ -336,9 +336,9 @@ 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, 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);
float x_thrust = math::constrain(get_control(0, 8), -1.0f, 1.0f);
float y_thrust = math::constrain(get_control(0, 9), -1.0f, 1.0f);
float z_thrust = math::constrain(get_control(0, 10), -1.0f, 1.0f);
// clean out class variable used to capture saturation
_saturation_status.value = 0;

26
src/modules/mc_rate_control/MulticopterRateControl.cpp
View File

@ -213,18 +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_body_sp(0) = _manual_control_sp.x;
_thrust_body_sp(1) = _manual_control_sp.y;
_thrust_body_sp(2) = _manual_control_sp.z;
_thrust_sp(0) = _manual_control_sp.x;
_thrust_sp(1) = _manual_control_sp.y;
_thrust_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] = _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.thrust_body[0] = _thrust_sp(0);
v_rates_sp.thrust_body[1] = _thrust_sp(1);
v_rates_sp.thrust_body[2] = _thrust_sp(2);
v_rates_sp.timestamp = hrt_absolute_time();
_v_rates_sp_pub.publish(v_rates_sp);
@ -238,9 +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_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];
_thrust_sp(0) = v_rates_sp.thrust_body[0];
_thrust_sp(1) = v_rates_sp.thrust_body[1];
_thrust_sp(2) = v_rates_sp.thrust_body[2];
}
}
@ -292,11 +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_body_sp(2)) ? -_thrust_body_sp(2) : 0.0f;
actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp(2)) ? -_thrust_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.control[actuator_controls_s::INDEX_X_THRUST] = (PX4_ISFINITE(_thrust_sp(0))) ? _thrust_sp(0) : 0.0f;
actuators.control[actuator_controls_s::INDEX_Y_THRUST] = (PX4_ISFINITE(_thrust_sp(1))) ? _thrust_sp(1) : 0.0f;
actuators.control[actuator_controls_s::INDEX_Z_THRUST] = (PX4_ISFINITE(_thrust_sp(2))) ? _thrust_sp(2) : 0.0f;
actuators.timestamp_sample = angular_velocity.timestamp_sample;
// scale effort by battery status if enabled

2
src/modules/mc_rate_control/MulticopterRateControl.hpp
View File

@ -129,7 +129,7 @@ private:
matrix::Vector3f _rates_sp; /**< angular rates setpoint */
matrix::Vector3f _thrust_body_sp; /**< 3-D thrust setpoint */
matrix::Vector3f _thrust_sp; /**< 3-D thrust setpoint */
bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */