mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-25 03:13:34 +08:00
vtol_att_control astyle
This commit is contained in:
committed by
Lorenz Meier
parent
3d1713f79e
commit
f58596bbcd
@@ -37,6 +37,7 @@ find \
|
|||||||
src/modules/systemlib \
|
src/modules/systemlib \
|
||||||
src/modules/unit_test \
|
src/modules/unit_test \
|
||||||
src/modules/uORB \
|
src/modules/uORB \
|
||||||
|
src/modules/vtol_att_control \
|
||||||
src/platforms \
|
src/platforms \
|
||||||
src/systemcmds \
|
src/systemcmds \
|
||||||
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) \
|
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) \
|
||||||
|
|||||||
@@ -155,7 +155,7 @@ void Standard::update_vtol_state()
|
|||||||
// transition to MC mode if transition time has passed
|
// transition to MC mode if transition time has passed
|
||||||
// XXX: base this on XY hold velocity of MC
|
// XXX: base this on XY hold velocity of MC
|
||||||
if (hrt_elapsed_time(&_vtol_schedule.transition_start) >
|
if (hrt_elapsed_time(&_vtol_schedule.transition_start) >
|
||||||
(_params_standard.back_trans_dur * 1000000.0f)) {
|
(_params_standard.back_trans_dur * 1000000.0f)) {
|
||||||
_vtol_schedule.flight_mode = MC_MODE;
|
_vtol_schedule.flight_mode = MC_MODE;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -236,7 +236,7 @@ void Standard::update_transition_state()
|
|||||||
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f)
|
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f)
|
||||||
) {
|
) {
|
||||||
float weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) /
|
float weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) /
|
||||||
_airspeed_trans_blend_margin;
|
_airspeed_trans_blend_margin;
|
||||||
_mc_roll_weight = weight;
|
_mc_roll_weight = weight;
|
||||||
_mc_pitch_weight = weight;
|
_mc_pitch_weight = weight;
|
||||||
_mc_yaw_weight = weight;
|
_mc_yaw_weight = weight;
|
||||||
@@ -300,18 +300,19 @@ void Standard::update_mc_state()
|
|||||||
|
|
||||||
// get projection of thrust vector on body x axis. This is used to
|
// get projection of thrust vector on body x axis. This is used to
|
||||||
// determine the desired forward acceleration which we want to achieve with the pusher
|
// determine the desired forward acceleration which we want to achieve with the pusher
|
||||||
math::Matrix<3,3> R(&_v_att->R[0]);
|
math::Matrix<3, 3> R(&_v_att->R[0]);
|
||||||
math::Matrix<3,3> R_sp(&_v_att_sp->R_body[0]);
|
math::Matrix<3, 3> R_sp(&_v_att_sp->R_body[0]);
|
||||||
math::Vector<3> thrust_sp_axis(-R_sp(0,2), -R_sp(1,2), -R_sp(2,2));
|
math::Vector<3> thrust_sp_axis(-R_sp(0, 2), -R_sp(1, 2), -R_sp(2, 2));
|
||||||
math::Vector<3> euler = R.to_euler();
|
math::Vector<3> euler = R.to_euler();
|
||||||
R.from_euler(0, 0, euler(2));
|
R.from_euler(0, 0, euler(2));
|
||||||
math::Vector<3> body_x_zero_tilt(R(0,0), R(1,0), R(2,0));
|
math::Vector<3> body_x_zero_tilt(R(0, 0), R(1, 0), R(2, 0));
|
||||||
|
|
||||||
// we are using a parameter to scale the thrust value in order to compensate for highly over/under-powered motors
|
// we are using a parameter to scale the thrust value in order to compensate for highly over/under-powered motors
|
||||||
_pusher_throttle = body_x_zero_tilt * thrust_sp_axis * _v_att_sp->thrust * _params_standard.forward_thurst_scale;
|
_pusher_throttle = body_x_zero_tilt * thrust_sp_axis * _v_att_sp->thrust * _params_standard.forward_thurst_scale;
|
||||||
_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
|
_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
|
||||||
|
|
||||||
float pitch_sp_corrected = _v_att_sp->pitch_body < -_params_standard.down_pitch_max ? -_params_standard.down_pitch_max : _v_att_sp->pitch_body;
|
float pitch_sp_corrected = _v_att_sp->pitch_body < -_params_standard.down_pitch_max ? -_params_standard.down_pitch_max :
|
||||||
|
_v_att_sp->pitch_body;
|
||||||
|
|
||||||
// compute new desired rotation matrix with corrected pitch angle
|
// compute new desired rotation matrix with corrected pitch angle
|
||||||
// and copy data to attitude setpoint topic
|
// and copy data to attitude setpoint topic
|
||||||
|
|||||||
@@ -103,7 +103,7 @@ private:
|
|||||||
} _vtol_schedule;
|
} _vtol_schedule;
|
||||||
|
|
||||||
bool _flag_enable_mc_motors;
|
bool _flag_enable_mc_motors;
|
||||||
float _pusher_throttle;
|
float _pusher_throttle;
|
||||||
float _airspeed_trans_blend_margin;
|
float _airspeed_trans_blend_margin;
|
||||||
|
|
||||||
void set_max_mc(unsigned pwm_value);
|
void set_max_mc(unsigned pwm_value);
|
||||||
|
|||||||
@@ -250,9 +250,9 @@ void Tailsitter::update_transition_state()
|
|||||||
/** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */
|
/** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */
|
||||||
if (_airspeed->indicated_airspeed_m_s <= _params_tailsitter.airspeed_trans) {
|
if (_airspeed->indicated_airspeed_m_s <= _params_tailsitter.airspeed_trans) {
|
||||||
_thrust_transition = _thrust_transition_start + (fabsf(THROTTLE_TRANSITION_MAX * _thrust_transition_start) *
|
_thrust_transition = _thrust_transition_start + (fabsf(THROTTLE_TRANSITION_MAX * _thrust_transition_start) *
|
||||||
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));
|
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));
|
||||||
_thrust_transition = math::constrain(_thrust_transition , _thrust_transition_start ,
|
_thrust_transition = math::constrain(_thrust_transition , _thrust_transition_start ,
|
||||||
(1.0f + THROTTLE_TRANSITION_MAX) * _thrust_transition_start);
|
(1.0f + THROTTLE_TRANSITION_MAX) * _thrust_transition_start);
|
||||||
_v_att_sp->thrust = _thrust_transition;
|
_v_att_sp->thrust = _thrust_transition;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -491,6 +491,7 @@ VtolAttitudeControl::is_fixed_wing_requested()
|
|||||||
if (_abort_front_transition) {
|
if (_abort_front_transition) {
|
||||||
if (to_fw) {
|
if (to_fw) {
|
||||||
to_fw = false;
|
to_fw = false;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
// the state changed to mc mode, reset the abort request
|
// the state changed to mc mode, reset the abort request
|
||||||
_abort_front_transition = false;
|
_abort_front_transition = false;
|
||||||
@@ -507,7 +508,7 @@ VtolAttitudeControl::is_fixed_wing_requested()
|
|||||||
void
|
void
|
||||||
VtolAttitudeControl::abort_front_transition()
|
VtolAttitudeControl::abort_front_transition()
|
||||||
{
|
{
|
||||||
if(!_abort_front_transition) {
|
if (!_abort_front_transition) {
|
||||||
mavlink_log_critical(&_mavlink_log_pub, "Front transition timeout occured, aborting");
|
mavlink_log_critical(&_mavlink_log_pub, "Front transition timeout occured, aborting");
|
||||||
_abort_front_transition = true;
|
_abort_front_transition = true;
|
||||||
_vtol_vehicle_status.vtol_transition_failsafe = true;
|
_vtol_vehicle_status.vtol_transition_failsafe = true;
|
||||||
@@ -789,8 +790,8 @@ void VtolAttitudeControl::task_main()
|
|||||||
|
|
||||||
/* Only publish if the proper mode(s) are enabled */
|
/* Only publish if the proper mode(s) are enabled */
|
||||||
if (_v_control_mode.flag_control_attitude_enabled ||
|
if (_v_control_mode.flag_control_attitude_enabled ||
|
||||||
_v_control_mode.flag_control_rates_enabled ||
|
_v_control_mode.flag_control_rates_enabled ||
|
||||||
_v_control_mode.flag_control_manual_enabled) {
|
_v_control_mode.flag_control_manual_enabled) {
|
||||||
if (_actuators_0_pub != nullptr) {
|
if (_actuators_0_pub != nullptr) {
|
||||||
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
|
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
|
||||||
|
|
||||||
|
|||||||
@@ -128,6 +128,7 @@ void VtolType::set_idle_fw()
|
|||||||
}
|
}
|
||||||
|
|
||||||
struct pwm_output_values pwm_values;
|
struct pwm_output_values pwm_values;
|
||||||
|
|
||||||
memset(&pwm_values, 0, sizeof(pwm_values));
|
memset(&pwm_values, 0, sizeof(pwm_values));
|
||||||
|
|
||||||
for (int i = 0; i < _params->vtol_motor_count; i++) {
|
for (int i = 0; i < _params->vtol_motor_count; i++) {
|
||||||
|
|||||||
Reference in New Issue
Block a user