mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-22 01:12:31 +00:00
ensure attitude setpoint initialization before arming
- On initialization _v_att_sp got filled with zeros leaving invalid quaternions - While not armed mc_pos_control did not publish any attitude setpoint which makes no sense - The attitude control just uses the data in _v_att_sp if it was (ever) updated or not
This commit is contained in:
@@ -389,6 +389,10 @@ _loop_update_rate_hz(initial_update_rate_hz)
|
||||
|
||||
_vehicle_status.is_rotary_wing = true;
|
||||
|
||||
/* initialize quaternions in messages to be valid */
|
||||
_v_att.q[0] = 1.f;
|
||||
_v_att_sp.q_d[0] = 1.f;
|
||||
|
||||
_params.rate_p.zero();
|
||||
_params.rate_i.zero();
|
||||
_params.rate_int_lim.zero();
|
||||
|
||||
@@ -3234,13 +3234,11 @@ MulticopterPositionControl::task_main()
|
||||
* in this case the attitude setpoint is published by the mavlink app.
|
||||
* - if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate
|
||||
* attitude setpoints for the transition).
|
||||
* - if not armed
|
||||
*/
|
||||
if (_control_mode.flag_armed &&
|
||||
(!(_control_mode.flag_control_offboard_enabled &&
|
||||
!(_control_mode.flag_control_position_enabled ||
|
||||
_control_mode.flag_control_velocity_enabled ||
|
||||
_control_mode.flag_control_acceleration_enabled)))) {
|
||||
if (!(_control_mode.flag_control_offboard_enabled &&
|
||||
!(_control_mode.flag_control_position_enabled ||
|
||||
_control_mode.flag_control_velocity_enabled ||
|
||||
_control_mode.flag_control_acceleration_enabled))) {
|
||||
|
||||
if (_att_sp_pub != nullptr) {
|
||||
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
|
||||
|
||||
Reference in New Issue
Block a user