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

vtol_type: calculate backtransition acceleration in forward direction

Browse Source 
- use track angle to calculate the current forward acceleration

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2020-03-09 11:53:24 +03:00
committed by Roman Bapst
parent 47b5d51369
commit 0184d8196f
1 changed files with 11 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/vtol_type.cpp
+11 -9
View File
@@ -191,27 +191,29 @@ float VtolType::update_and_get_backtransition_pitch_sp()
{
// maximum up or down pitch the controller is allowed to demand
const float pitch_lim = 0.3f;
const Eulerf euler(Quatf(_v_att->q));
// calculate acceleration in body x direction
const Dcmf R_to_body(Quatf(_v_att->q).inversed());
const Vector3f acc = R_to_body * Vector3f(_local_pos->ax, _local_pos->ay, _local_pos->az);
float accel_body_x = acc(0);
float accel_error_x = 0.0f;
const float track = atan2f(_local_pos->vy, _local_pos->vx);
const float accel_body_forward = cosf(track) * _local_pos->ax + sinf(track) * _local_pos->ay;
float accel_error_forward = 0.0f;
// get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number)
accel_error_x = _params->back_trans_dec_sp + accel_body_x;
accel_error_forward = _params->back_trans_dec_sp + accel_body_forward;
float pitch_sp_new = _params->dec_to_pitch_ff * _params->back_trans_dec_sp + _accel_to_pitch_integ;
float integrator_input = _params->dec_to_pitch_i * accel_error_x;
float integrator_input = _params->dec_to_pitch_i * accel_error_forward;
if ((pitch_sp_new >= pitch_lim && accel_error_x > 0.0f) ||
(pitch_sp_new <= -pitch_lim && accel_error_x < 0.0f)
if ((pitch_sp_new >= pitch_lim && accel_error_forward > 0.0f) ||
(pitch_sp_new <= -pitch_lim && accel_error_forward < 0.0f)
) {
integrator_input = 0.0f;
}
_accel_to_pitch_integ += integrator_input * _transition_dt;
return math::constrain(pitch_sp_new, -pitch_lim, pitch_lim);
}