PX4 Firmware
PX4 Autopilot Software http://px4.io
AttitudeControlTest.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #include <gtest/gtest.h>
35 #include <AttitudeControl.hpp>
37 
38 using namespace matrix;
39 
40 TEST(AttitudeControlTest, AllZeroCase)
41 {
42  AttitudeControl attitude_control;
43  Vector3f rate_setpoint = attitude_control.update(Quatf(), Quatf(), 0.f);
44  EXPECT_EQ(rate_setpoint, Vector3f());
45 }
46 
47 class AttitudeControlConvergenceTest : public ::testing::Test
48 {
49 public:
51  {
52  _attitude_control.setProportionalGain(Vector3f(.5f, .6f, .3f));
53  _attitude_control.setRateLimit(Vector3f(100, 100, 100));
54  }
55 
57  {
58  int i; // need function scope to check how many steps
59  Vector3f rate_setpoint(1000, 1000, 1000);
60 
61  for (i = 100; i > 0; i--) {
62  // run attitude control to get rate setpoints
63  const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state, _quat_goal, 0.f);
64  // rotate the simulated state quaternion according to the rate setpoint
65  _quat_state = _quat_state * Quatf(AxisAnglef(rate_setpoint_new));
66 
67  // expect the error and hence also the output to get smaller with each iteration
68  if (rate_setpoint_new.norm() >= rate_setpoint.norm()) {
69  break;
70  }
71 
72  rate_setpoint = rate_setpoint_new;
73  }
74 
75  EXPECT_EQ(adaptAntipodal(_quat_state), adaptAntipodal(_quat_goal));
76  // it shouldn't have taken longer than an iteration timeout to converge
77  EXPECT_GT(i, 0);
78  }
79 
81  {
82  for (int i = 0; i < 4; i++) {
83  if (fabs(q(i)) > FLT_EPSILON) {
84  return q * math::sign(q(i));
85  }
86  }
87 
88  return q;
89  }
90 
94 };
95 
96 TEST_F(AttitudeControlConvergenceTest, AttitudeControlConvergence)
97 {
98  const int inputs = 8;
99 
100  const Quatf QArray[inputs] = {
101  Quatf(),
102  Quatf(0, 1, 0, 0),
103  Quatf(0, 0, 1, 0),
104  Quatf(0, 0, 0, 1),
105  Quatf(0.698f, 0.024f, -0.681f, -0.220f),
106  Quatf(-0.820f, -0.313f, 0.225f, -0.423f),
107  Quatf(0.599f, -0.172f, 0.755f, -0.204f),
108  Quatf(0.216f, -0.662f, 0.290f, -0.656f)
109  };
110 
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];
116  _quat_state.normalize();
117  _quat_goal.normalize();
118  checkConvergence();
119  }
120  }
121 }
TEST_F(AttitudeControlConvergenceTest, AttitudeControlConvergence)
matrix::Vector3f update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward)
Run one control loop cycle calculation.
void normalize()
Definition: Vector.hpp:87
TEST(AttitudeControlTest, AllZeroCase)
#define FLT_EPSILON
Quaternion class.
Definition: Dcm.hpp:24
collection of rather simple mathematical functions that get used over and over again ...
A quaternion based attitude controller.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
int sign(T val)
Definition: Functions.hpp:49
Vector3< float > Vector3f
Definition: Vector3.hpp:136
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
AxisAngle< float > AxisAnglef
Definition: AxisAngle.hpp:160
Type norm() const
Definition: Vector.hpp:73