Skip to content

Commit

Permalink
ensure attitude setpoint initialization before arming
Browse files Browse the repository at this point in the history
- 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
  • Loading branch information
MaEtUgR committed Mar 15, 2018
1 parent e32b04f commit ff25c7f
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 6 deletions.
4 changes: 4 additions & 0 deletions src/modules/mc_att_control/mc_att_control_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
10 changes: 4 additions & 6 deletions src/modules/mc_pos_control/mc_pos_control_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down

0 comments on commit ff25c7f

Please sign in to comment.