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

VTOL rate control architecture improvements (#10819)

Browse Source 
* attitude and rate setpoint message: use 3D array for thrust demand
* FixedWingAttitudeControl: rework airspeed scaling
  * move airspeed and scaling calculation into separate method
  * if vtol in hover and airspeed disabled use minimum airspeed instead of trim airspeed
This commit is contained in:
Roman Bapst
2018-11-21 20:32:40 -05:00
committed by Daniel Agar
parent 75c1396ed7
commit 90bfdb6f0a
28 changed files with 387 additions and 414 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control.hpp
+2 -2
View File
@@ -166,7 +166,6 @@ private:
orb_advert_t _controller_status_pub{nullptr}; /**< controller status publication */
orb_advert_t _vehicle_attitude_setpoint_pub{nullptr};
orb_id_t _rates_sp_id{nullptr}; /**< pointer to correct rates setpoint uORB metadata structure */
orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */
bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */
@@ -196,8 +195,9 @@ private:
matrix::Vector3f _rates_prev_filtered; /**< angular rates on previous step (low-pass filtered) */
matrix::Vector3f _rates_sp; /**< angular rates setpoint */
matrix::Vector3f _rates_int; /**< angular rates integral error */
float _thrust_sp; /**< thrust setpoint */
matrix::Vector3f _att_control; /**< attitude control vector */
float _thrust_sp{0.0f}; /**< thrust setpoint */
matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
src/modules/mc_att_control/mc_att_control_main.cpp
+13 -11
View File
@@ -273,13 +273,11 @@ MulticopterAttitudeControl::vehicle_status_poll()
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (_rates_sp_id == nullptr) {
if (_actuators_id == nullptr) {
if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
} else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
@@ -508,7 +506,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
q_sp.copyTo(attitude_setpoint.q_d);
attitude_setpoint.q_d_valid = true;
attitude_setpoint.thrust = throttle_curve(_manual_control_sp.z);
attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
attitude_setpoint.landing_gear = get_landing_gear_state();
attitude_setpoint.timestamp = hrt_absolute_time();
@@ -524,7 +522,9 @@ void
MulticopterAttitudeControl::control_attitude()
{
vehicle_attitude_setpoint_poll();
_thrust_sp = _v_att_sp.thrust;
// physical thrust axis is the negative of body z axis
_thrust_sp = -_v_att_sp.thrust_body[2];
/* prepare yaw weight from the ratio between roll/pitch and yaw gains */
Vector3f attitude_gain = _attitude_p;
@@ -607,7 +607,7 @@ Vector3f
MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate)
{
/* throttle pid attenuation factor */
float tpa = 1.0f - tpa_rate * (fabsf(_v_rates_sp.thrust) - tpa_breakpoint) / (1.0f - tpa_breakpoint);
float tpa = 1.0f - tpa_rate * (fabsf(_thrust_sp) - tpa_breakpoint) / (1.0f - tpa_breakpoint);
tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa));
Vector3f pidAttenuationPerAxis;
@@ -731,9 +731,11 @@ MulticopterAttitudeControl::publish_rates_setpoint()
_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 = _thrust_sp;
_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.timestamp = hrt_absolute_time();
orb_publish_auto(_rates_sp_id, &_v_rates_sp_pub, &_v_rates_sp, nullptr, ORB_PRIO_DEFAULT);
orb_publish_auto(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp_pub, &_v_rates_sp, nullptr, ORB_PRIO_DEFAULT);
}
void
@@ -886,7 +888,7 @@ MulticopterAttitudeControl::run()
bool attitude_setpoint_generated = false;
if (_v_control_mode.flag_control_attitude_enabled) {
if (_v_control_mode.flag_control_attitude_enabled && _vehicle_status.is_rotary_wing) {
if (attitude_updated) {
// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
if (_v_control_mode.flag_control_manual_enabled &&
@@ -903,7 +905,7 @@ MulticopterAttitudeControl::run()
} else {
/* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) {
if (_v_control_mode.flag_control_manual_enabled && _vehicle_status.is_rotary_wing) {
if (manual_control_updated) {
/* manual rates control - ACRO mode */
Vector3f man_rate_sp(
@@ -921,7 +923,7 @@ MulticopterAttitudeControl::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;
_thrust_sp = -_v_rates_sp.thrust_body[2];
}
}
}