2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
25 #include "build/debug.h"
27 #include "common/axis.h"
28 #include "common/maths.h"
30 #include "config/feature.h"
32 #include "pg/pg_ids.h"
35 #include "drivers/accgyro/accgyro.h"
36 #include "drivers/compass/compass.h"
37 #include "drivers/sensor.h"
39 #include "fc/rc_controls.h"
40 #include "fc/rc_modes.h"
41 #include "fc/runtime_config.h"
43 #include "flight/mixer.h"
44 #include "flight/pid.h"
45 #include "flight/imu.h"
51 #include "sensors/acceleration.h"
52 #include "sensors/barometer.h"
53 #include "sensors/compass.h"
54 #include "sensors/gyro.h"
55 #include "sensors/sensors.h"
57 void imuComputeRotationMatrix(void);
58 void imuUpdateEulerAngles(void);
61 extern float rMat
[3][3];
62 extern bool attitudeIsEstablished
;
64 PG_REGISTER(rcControlsConfig_t
, rcControlsConfig
, PG_RC_CONTROLS_CONFIG
, 0);
65 PG_REGISTER(barometerConfig_t
, barometerConfig
, PG_BAROMETER_CONFIG
, 0);
67 PG_RESET_TEMPLATE(featureConfig_t
, featureConfig
,
72 #include "unittest_macros.h"
73 #include "gtest/gtest.h"
75 const float sqrt2over2
= sqrt(2) / 2.0f
;
77 TEST(FlightImuTest
, TestCalculateRotationMatrix
)
87 imuComputeRotationMatrix();
89 EXPECT_FLOAT_EQ(1.0f
, rMat
[0][0]);
90 EXPECT_FLOAT_EQ(0.0f
, rMat
[0][1]);
91 EXPECT_FLOAT_EQ(0.0f
, rMat
[0][2]);
92 EXPECT_FLOAT_EQ(0.0f
, rMat
[1][0]);
93 EXPECT_FLOAT_EQ(1.0f
, rMat
[1][1]);
94 EXPECT_FLOAT_EQ(0.0f
, rMat
[1][2]);
95 EXPECT_FLOAT_EQ(0.0f
, rMat
[2][0]);
96 EXPECT_FLOAT_EQ(0.0f
, rMat
[2][1]);
97 EXPECT_FLOAT_EQ(1.0f
, rMat
[2][2]);
99 // 90 degrees around Z axis
105 imuComputeRotationMatrix();
107 EXPECT_NEAR(0.0f
, rMat
[0][0], TOL
);
108 EXPECT_NEAR(-1.0f
, rMat
[0][1], TOL
);
109 EXPECT_NEAR(0.0f
, rMat
[0][2], TOL
);
110 EXPECT_NEAR(1.0f
, rMat
[1][0], TOL
);
111 EXPECT_NEAR(0.0f
, rMat
[1][1], TOL
);
112 EXPECT_NEAR(0.0f
, rMat
[1][2], TOL
);
113 EXPECT_NEAR(0.0f
, rMat
[2][0], TOL
);
114 EXPECT_NEAR(0.0f
, rMat
[2][1], TOL
);
115 EXPECT_NEAR(1.0f
, rMat
[2][2], TOL
);
117 // 60 degrees around X axis
123 imuComputeRotationMatrix();
125 EXPECT_NEAR(1.0f
, rMat
[0][0], TOL
);
126 EXPECT_NEAR(0.0f
, rMat
[0][1], TOL
);
127 EXPECT_NEAR(0.0f
, rMat
[0][2], TOL
);
128 EXPECT_NEAR(0.0f
, rMat
[1][0], TOL
);
129 EXPECT_NEAR(0.5f
, rMat
[1][1], TOL
);
130 EXPECT_NEAR(-0.866f
, rMat
[1][2], TOL
);
131 EXPECT_NEAR(0.0f
, rMat
[2][0], TOL
);
132 EXPECT_NEAR(0.866f
, rMat
[2][1], TOL
);
133 EXPECT_NEAR(0.5f
, rMat
[2][2], TOL
);
136 TEST(FlightImuTest
, TestUpdateEulerAngles
)
139 memset(rMat
, 0.0, sizeof(float) * 9);
141 imuUpdateEulerAngles();
143 EXPECT_EQ(0, attitude
.values
.roll
);
144 EXPECT_EQ(0, attitude
.values
.pitch
);
145 EXPECT_EQ(0, attitude
.values
.yaw
);
148 memset(rMat
, 0.0, sizeof(float) * 9);
149 rMat
[0][0] = sqrt2over2
;
150 rMat
[0][1] = sqrt2over2
;
151 rMat
[1][0] = -sqrt2over2
;
152 rMat
[1][1] = sqrt2over2
;
154 imuUpdateEulerAngles();
156 EXPECT_EQ(0, attitude
.values
.roll
);
157 EXPECT_EQ(0, attitude
.values
.pitch
);
158 EXPECT_EQ(450, attitude
.values
.yaw
);
161 TEST(FlightImuTest
, TestSmallAngle
)
163 const float r1
= 0.898;
164 const float r2
= 0.438;
167 imuConfigMutable()->small_angle
= 25;
169 attitudeIsEstablished
= true;
172 memset(rMat
, 0.0, sizeof(float) * 9);
175 imuComputeRotationMatrix();
179 EXPECT_EQ(false, isUpright());
181 EXPECT_TRUE(isUpright());
182 >>>>>>> Improved tests
.
191 imuComputeRotationMatrix();
194 EXPECT_FALSE(isUpright());
197 memset(rMat
, 0.0, sizeof(float) * 9);
200 imuComputeRotationMatrix();
203 EXPECT_FALSE(isUpright());
209 boxBitmask_t rcModeActivationMask
;
211 int16_t rcData
[MAX_SUPPORTED_RC_CHANNEL_COUNT
];
217 gpsSolutionData_t gpsSol
;
218 int16_t GPS_verticalSpeedInCmS
;
221 int16_t debug
[DEBUG16_VALUE_COUNT
];
224 uint16_t flightModeFlags
;
227 pidProfile_t
*currentPidProfile
;
229 uint16_t enableFlightMode(flightModeFlags_e mask
)
231 return flightModeFlags
|= (mask
);
234 uint16_t disableFlightMode(flightModeFlags_e mask
)
236 return flightModeFlags
&= ~(mask
);
239 bool sensors(uint32_t mask
)
245 uint32_t millis(void) { return 0; }
246 uint32_t micros(void) { return 0; }
248 bool compassIsHealthy(void) { return true; }
249 bool baroIsCalibrationComplete(void) { return true; }
250 void performBaroCalibrationCycle(void) {}
251 int32_t baroCalculateAltitude(void) { return 0; }
252 bool gyroGetAccumulationAverage(float *) { return false; }
253 bool accGetAccumulationAverage(float *) { return false; }
254 void mixerSetThrottleAngleCorrection(int) {};
255 bool gpsRescueIsRunning(void) { return false; }
256 bool isFixedWing(void) { return false; }