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:
Matthias Grob
2018-03-13 10:23:17 +01:00
parent e32b04fff1
commit ff25c7f48a
2 changed files with 8 additions and 6 deletions

View File

@@ -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();

View File

@@ -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);