34 #include <gtest/gtest.h> 40 TEST(AttitudeControlTest, AllZeroCase)
44 EXPECT_EQ(rate_setpoint,
Vector3f());
52 _attitude_control.setProportionalGain(
Vector3f(.5
f, .6
f, .3
f));
53 _attitude_control.setRateLimit(
Vector3f(100, 100, 100));
59 Vector3f rate_setpoint(1000, 1000, 1000);
61 for (i = 100; i > 0; i--) {
63 const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state, _quat_goal, 0.
f);
68 if (rate_setpoint_new.
norm() >= rate_setpoint.
norm()) {
72 rate_setpoint = rate_setpoint_new;
75 EXPECT_EQ(adaptAntipodal(_quat_state), adaptAntipodal(_quat_goal));
82 for (
int i = 0; i < 4; i++) {
100 const Quatf QArray[inputs] = {
106 Quatf(-0.820
f, -0.313
f, 0.225
f, -0.423
f),
111 for (
int i = 0; i < inputs; i++) {
112 for (
int j = 0; j < inputs; j++) {
113 printf(
"--- Input combination: %d %d\n", i, j);
114 _quat_state = QArray[i];
115 _quat_goal = QArray[j];
117 _quat_goal.normalize();
TEST_F(AttitudeControlConvergenceTest, AttitudeControlConvergence)
matrix::Vector3f update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward)
Run one control loop cycle calculation.
TEST(AttitudeControlTest, AllZeroCase)
AttitudeControl _attitude_control
collection of rather simple mathematical functions that get used over and over again ...
A quaternion based attitude controller.
Quatf adaptAntipodal(const Quatf q)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Vector3< float > Vector3f
AttitudeControlConvergenceTest()
Quaternion< float > Quatf
AxisAngle< float > AxisAnglef