AttitudeControlTest: add scale testing for yaw weight

src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp

@@ -49,14 +49,14 @@ class AttitudeControlConvergenceTest : public ::testing::Test
 public:
 	AttitudeControlConvergenceTest()
 	{
-		_attitude_control.setProportionalGain(Vector3f(.5f, .6f, .3f));
-		_attitude_control.setRateLimit(Vector3f(100, 100, 100));
+		_attitude_control.setProportionalGain(Vector3f(.5f, .6f, .3f), .4f);
+		_attitude_control.setRateLimit(Vector3f(100.f, 100.f, 100.f));
 	}
 
 	void checkConvergence()
 	{
 		int i; // need function scope to check how many steps
-		Vector3f rate_setpoint(1000, 1000, 1000);
+		Vector3f rate_setpoint(1000.f, 1000.f, 1000.f);
 
 		for (i = 100; i > 0; i--) {
 			// run attitude control to get rate setpoints
@@ -109,3 +109,28 @@ TEST_F(AttitudeControlConvergenceTest, AttitudeControlConvergence)
 		}
 	}
 }
+
+TEST(AttitudeControlTest, YawWeightScaling)
+{
+	// GIVEN: default tuning
+	AttitudeControl attitude_control;
+	const float yaw_gain = 2.8f;
+	const float yaw_sp = .1f;
+	attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), .4f);
+	attitude_control.setRateLimit(Vector3f(1000.f, 1000.f, 1000.f));
+
+	// WHEN: we command a pure yaw turn
+	Vector3f rate_setpoint = attitude_control.update(Quatf(), Quatf(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)), 0.f);
+
+	// THEN: no actuation in roll, pitch
+	EXPECT_EQ(Vector2f(rate_setpoint), Vector2f());
+	// THEN: actuation error * gain in yaw
+	EXPECT_NEAR(rate_setpoint(2), yaw_sp * yaw_gain, 1e-4f);
+
+	// GIVEN: corner case of zero yaw weight
+	attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), 0.f);
+	// WHEN: we command a pure yaw turn
+	rate_setpoint = attitude_control.update(Quatf(), Quatf(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)), 0.f);
+	// THEN: no actuation (also no NAN)
+	EXPECT_EQ(rate_setpoint, Vector3f());
+}