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

mc_att_control: respect spoolup time in Stabilized mode

Browse Source
This commit is contained in:
Matthias Grob 2023-08-28 19:59:41 +02:00
parent 24111df176
commit 4ca366c04f
2 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/mc_att_control/mc_att_control.hpp
View File

@ -128,6 +128,7 @@ private:
hrt_abstime _last_run{0};
hrt_abstime _last_attitude_setpoint{0};
bool _spooled_up{false}; ///< used to make sure the vehicle cannot take off during the spoolup time
bool _landed{true};
bool _reset_yaw_sp{true};
bool _heading_good_for_control{true}; ///< initialized true to have heading lock when local position never published
@ -157,7 +158,9 @@ private:
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min, /**< minimum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max, /**< maximum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover, /**< throttle at stationary hover */
(ParamInt<px4::params::MPC_THR_CURVE>) _param_mpc_thr_curve /**< throttle curve behavior */
(ParamInt<px4::params::MPC_THR_CURVE>) _param_mpc_thr_curve, /**< throttle curve behavior */
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
)
};

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

@ -179,7 +179,11 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
attitude_setpoint.pitch_body = euler_sp(1);
attitude_setpoint.yaw_body = euler_sp(2);
attitude_setpoint.thrust_body[2] = -throttle_curve((_manual_control_setpoint.throttle + 1.f) * .5f);
// Only set thrust setpoint different from idle once the vehicle is spooled up
if (_spooled_up) {
attitude_setpoint.thrust_body[2] = -throttle_curve((_manual_control_setpoint.throttle + 1.f) * .5f);
}
attitude_setpoint.timestamp = hrt_absolute_time();
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
@ -263,6 +267,8 @@ MulticopterAttitudeControl::Run()
_vtol_in_transition_mode = vehicle_status.in_transition_mode;
_vtol_tailsitter = vehicle_status.is_vtol_tailsitter;
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
_spooled_up = armed && hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s;
}
}