VTOL rate control architecture improvements (#10819)

* attitude and rate setpoint message: use 3D array for thrust demand
* FixedWingAttitudeControl: rework airspeed scaling
  * move airspeed and scaling calculation into separate method
  * if vtol in hover and airspeed disabled use minimum airspeed instead of trim airspeed

src/modules/vtol_att_control/standard.cpp

@@ -46,13 +46,10 @@
 
 #include <float.h>
 
-using matrix::wrap_pi;
+using namespace matrix;
 
 Standard::Standard(VtolAttitudeControl *attc) :
-	VtolType(attc),
-	_pusher_throttle(0.0f),
-	_reverse_output(0.0f),
-	_airspeed_trans_blend_margin(0.0f)
+	VtolType(attc)
 {
 	_vtol_schedule.flight_mode = MC_MODE;
 	_vtol_schedule.transition_start = 0;
@@ -156,8 +153,8 @@ void Standard::update_vtol_state()
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
 			// transition to MC mode if transition time has passed or forward velocity drops below MPC cruise speed
 
-			const matrix::Dcmf R_to_body(matrix::Quatf(_v_att->q).inversed());
-			const matrix::Vector3f vel = R_to_body * matrix::Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
+			const Dcmf R_to_body(Quatf(_v_att->q).inversed());
+			const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
 
 			float x_vel = vel(0);
 
@@ -261,7 +258,7 @@ void Standard::update_transition_state()
 
 		// ramp up FW_PSP_OFF
 		_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight);
-		matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
+		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;
 
@@ -277,7 +274,7 @@ void Standard::update_transition_state()
 
 		// maintain FW_PSP_OFF
 		_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset;
-		matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
+		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;
 
@@ -333,18 +330,18 @@ void Standard::update_mc_state()
 		return;
 	}
 
-	matrix::Dcmf R(matrix::Quatf(_v_att->q));
-	matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d));
-	matrix::Eulerf euler(R);
-	matrix::Eulerf euler_sp(R_sp);
+	const Dcmf R(Quatf(_v_att->q));
+	const Dcmf R_sp(Quatf(_v_att_sp->q_d));
+	const Eulerf euler(R);
+	const Eulerf euler_sp(R_sp);
 	_pusher_throttle = 0.0f;
 
 	// direction of desired body z axis represented in earth frame
-	matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+	Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
 
 	// rotate desired body z axis into new frame which is rotated in z by the current
 	// heading of the vehicle. we refer to this as the heading frame.
-	matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2));
+	Dcmf R_yaw = Eulerf(0.0f, 0.0f, -euler(2));
 	body_z_sp = R_yaw * body_z_sp;
 	body_z_sp.normalize();
 
@@ -365,25 +362,25 @@ void Standard::update_mc_state()
 		float pitch_new = 0.0f;
 
 		// create corrected desired body z axis in heading frame
-		matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f);
-		matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));
+		const Dcmf R_tmp = Eulerf(roll_new, pitch_new, 0.0f);
+		Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));
 
 		// rotate the vector into a new frame which is rotated in z by the desired heading
 		// with respect to the earh frame.
 		const float yaw_error = wrap_pi(euler_sp(2) - euler(2));
-		matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error);
+		const Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error);
 		tilt_new = R_yaw_correction * tilt_new;
 
 		// now extract roll and pitch setpoints
 		_v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2));
 		_v_att_sp->roll_body = -asinf(tilt_new(1));
-		R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2));
-		matrix::Quatf q_sp(R_sp);
+
+		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;
 	}
 
 	_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
-
 }
 
 void Standard::update_fw_state()
@@ -475,5 +472,5 @@ void
 Standard::waiting_on_tecs()
 {
 	// keep thrust from transition
-	_v_att_sp->thrust = _pusher_throttle;
+	_v_att_sp->thrust_body[0] = _pusher_throttle;
 };