Browse Source

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
sbg
Matthias Grob 7 years ago
parent
commit
ff25c7f48a
  1. 4
      src/modules/mc_att_control/mc_att_control_main.cpp
  2. 10
      src/modules/mc_pos_control/mc_pos_control_main.cpp

4
src/modules/mc_att_control/mc_att_control_main.cpp

@ -389,6 +389,10 @@ _loop_update_rate_hz(initial_update_rate_hz) @@ -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();

10
src/modules/mc_pos_control/mc_pos_control_main.cpp

@ -3234,13 +3234,11 @@ MulticopterPositionControl::task_main() @@ -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);

Loading…
Cancel
Save