vehicle_attitude_setpoint: get rid of unused q_d_valid flag

msg/vehicle_attitude_setpoint.msg

@@ -8,7 +8,6 @@ float32 yaw_sp_move_rate	# rad/s (commanded by user)
 
 # For quaternion-based attitude control
 float32[4] q_d			# Desired quaternion for quaternion control
-bool q_d_valid			# Set to true if quaternion vector is valid
 
 # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand.
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.

src/examples/fixedwing_control/main.cpp

@@ -208,13 +208,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct p
 	}
 
 	matrix::Eulerf att_spe(roll_body, 0, bearing);
-
-	matrix::Quatf qd(att_spe);
-
-	att_sp->q_d[0] = qd(0);
-	att_sp->q_d[1] = qd(1);
-	att_sp->q_d[2] = qd(2);
-	att_sp->q_d[3] = qd(3);
+	matrix::Quatf(att_spe).copyTo(att_sp->q_d);
 }
 
 int parameters_init(struct param_handles *handles)

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -169,7 +169,6 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 
 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 					q.copyTo(_att_sp.q_d);
-					_att_sp.q_d_valid = true;
 
 					_att_sp.timestamp = hrt_absolute_time();
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1559,7 +1559,6 @@ FixedwingPositionControl::Run()
 
 			Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 			q.copyTo(_att_sp.q_d);
-			_att_sp.q_d_valid = true;
 
 			if (_control_mode.flag_control_offboard_enabled ||
 			    _control_mode.flag_control_position_enabled ||

src/modules/mavlink/mavlink_messages.cpp

@@ -3516,17 +3516,7 @@ protected:
 			mavlink_attitude_target_t msg = {};
 
 			msg.time_boot_ms = att_sp.timestamp / 1000;
-
-			if (att_sp.q_d_valid) {
-				memcpy(&msg.q[0], &att_sp.q_d[0], sizeof(msg.q));
-
-			} else {
-				matrix::Quatf q = matrix::Eulerf(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body);
-
-				for (size_t i = 0; i < 4; i++) {
-					msg.q[i] = q(i);
-				}
-			}
+			matrix::Quatf(att_sp.q_d).copyTo(msg.q);
 
 			msg.body_roll_rate = att_rates_sp.roll;
 			msg.body_pitch_rate = att_rates_sp.pitch;

src/modules/mavlink/mavlink_receiver.cpp

@@ -1491,7 +1491,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 					if (!ignore_attitude_msg) { // only copy att sp if message contained new data
 						matrix::Quatf q(set_attitude_target.q);
 						q.copyTo(att_sp.q_d);
-						att_sp.q_d_valid = true;
 
 						matrix::Eulerf euler{q};
 						att_sp.roll_body = euler.phi();

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -214,7 +214,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
 	/* copy quaternion setpoint to attitude setpoint topic */
 	Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
 	q_sp.copyTo(attitude_setpoint.q_d);
-	attitude_setpoint.q_d_valid = true;
 
 	attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
 	attitude_setpoint.timestamp = hrt_absolute_time();

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -94,7 +94,6 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
 	// copy quaternion setpoint to attitude setpoint topic
 	Quatf q_sp = R_sp;
 	q_sp.copyTo(att_sp.q_d);
-	att_sp.q_d_valid = true;
 
 	// calculate euler angles, for logging only, must not be used for control
 	Eulerf euler = R_sp;

src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp

@@ -62,7 +62,6 @@ TEST(PositionControlTest, EmptySetpoint)
 	EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f);
 	EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f);
 	EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f));
-	//EXPECT_EQ(attitude.q_d_valid, false); // TODO should not be true when there was no control
 	EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f));
 	EXPECT_EQ(attitude.roll_reset_integral, false);
 	EXPECT_EQ(attitude.pitch_reset_integral, false);

src/modules/navigator/gpsfailure.cpp

@@ -91,7 +91,6 @@ GpsFailure::on_active()
 
 			Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f));
 			q.copyTo(att_sp.q_d);
-			att_sp.q_d_valid = true;
 
 			if (_navigator->get_vstatus()->is_vtol) {
 				_fw_virtual_att_sp_pub.publish(att_sp);

src/modules/vtol_att_control/standard.cpp

@@ -266,7 +266,6 @@ void Standard::update_transition_state()
 		_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight);
 		const Quatf q_sp(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->front_trans_timeout > FLT_EPSILON) {
@@ -282,7 +281,6 @@ void Standard::update_transition_state()
 		_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset;
 		const Quatf q_sp(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;
 
 		_pusher_throttle = 0.0f;
 

src/modules/vtol_att_control/tailsitter.cpp

@@ -261,7 +261,6 @@ void Tailsitter::update_transition_state()
 	_v_att_sp->yaw_body = euler_sp.psi();
 
 	_q_trans_sp.copyTo(_v_att_sp->q_d);
-	_v_att_sp->q_d_valid = true;
 }
 
 void Tailsitter::waiting_on_tecs()

src/modules/vtol_att_control/vtol_type.cpp

@@ -455,7 +455,6 @@ float VtolType::pusher_assist()
 
 		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2)));
 		q_sp.copyTo(_v_att_sp->q_d);
-		_v_att_sp->q_d_valid = true;
 	}
 
 	return forward_thrust;