diff --git a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp index dfe98499fb..3f67c23f51 100644 --- a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp +++ b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp @@ -64,9 +64,8 @@ bool FlightTaskTransition::update() // demand zero vertical velocity and level attitude // tailsitters will override attitude and thrust setpoint // tiltrotors and standard vtol will overrride roll and pitch setpoint but keep vertical thrust setpoint - _position_setpoint *= NAN; + _position_setpoint.setAll(NAN); _velocity_setpoint(2) = 0.0f; - _position_setpoint(2) = NAN; _yaw_setpoint = NAN; return true; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index a66bbcb695..376f757a79 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -196,18 +196,15 @@ float VtolType::update_and_get_backtransition_pitch_sp() 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_forward = _params->back_trans_dec_sp + accel_body_forward; + const float 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; + const 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_forward; if ((pitch_sp_new >= pitch_lim && accel_error_forward > 0.0f) || - (pitch_sp_new <= -pitch_lim && accel_error_forward < 0.0f) - ) { + (pitch_sp_new <= -pitch_lim && accel_error_forward < 0.0f)) { integrator_input = 0.0f; } @@ -215,7 +212,6 @@ float VtolType::update_and_get_backtransition_pitch_sp() return math::constrain(pitch_sp_new, -pitch_lim, pitch_lim); - } bool VtolType::can_transition_on_ground() diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 93eb2b3666..f4faaf8f39 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -226,7 +226,7 @@ protected: motor_state _motor_state = motor_state::DISABLED; hrt_abstime _last_loop_ts = 0; - float _transition_dt = 0; + float _transition_dt = 0; float _accel_to_pitch_integ = 0;