Simplify quaternion calc

PX4 · Mar 6, 2017 · 76c43ee · 76c43ee
1 parent 46f445a
commit 76c43ee
Showing 1 changed file with 6 additions and 11 deletions.
diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
@@ -293,13 +293,10 @@ void Standard::update_transition_state()
 		}
 
 		// ramp up FW_PSP_OFF
-		matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d));
 		_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - _mc_pitch_weight);
-		R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body);
-		matrix::Quatf q_sp(R_sp);
-		memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d));
-
-
+		matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
+		q_sp.copyTo(_v_att_sp->q_d);
+		_v_att_sp->q_d_valid = true;
 
 		// check front transition timeout
 		if (_params_standard.front_trans_timeout > FLT_EPSILON) {
@@ -312,12 +309,10 @@ void Standard::update_transition_state()
 	} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
 
 		// maintain FW_PSP_OFF
-		matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d));
 		_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset;
-		R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body);
-		matrix::Quatf q_sp(R_sp);
-		memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d));
-
+		matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
+		q_sp.copyTo(_v_att_sp->q_d);
+		_v_att_sp->q_d_valid = true;
 
 		// continually increase mc attitude control as we transition back to mc mode
 		if (_params_standard.back_trans_dur > 0.0f) {