Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +03:00
Code Issues Wiki Activity

Improved detection of upright / 'SMALL_ANGLE' state.

Browse source
This commit is contained in:
mikeller 2019-11-17 13:40:30 +13:00 • committed by Michael Keller
parent 6694d4ebc8
commit cc8b8d3bf6
11 changed files with 108 additions and 95 deletions
Download patch file Download diff file Expand all files Collapse all files

15
src/test/unit/flight_imu_unittest.cc
View file

@ -59,6 +59,7 @@ extern "C" {
extern quaternion q;
extern float rMat[3][3];
extern bool attitudeIsEstablished;
PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0);
@ -164,15 +165,17 @@ TEST(FlightImuTest, TestSmallAngle)
// given
imuConfigMutable()->small_angle = 25;
imuConfigure(0, 0);
attitudeIsEstablished = true;
// and
memset(rMat, 0.0, sizeof(float) * 9);
// when
imuUpdateEulerAngles();
imuComputeRotationMatrix();
// expect
EXPECT_EQ(0, STATE(SMALL_ANGLE));
EXPECT_EQ(true, isUpright());
// given
rMat[0][0] = r1;
@ -181,19 +184,19 @@ TEST(FlightImuTest, TestSmallAngle)
rMat[2][2] = r1;
// when
imuUpdateEulerAngles();
imuComputeRotationMatrix();
// expect
EXPECT_EQ(SMALL_ANGLE, STATE(SMALL_ANGLE));
EXPECT_EQ(true, isUpright());
// given
memset(rMat, 0.0, sizeof(float) * 9);
// when
imuUpdateEulerAngles();
imuComputeRotationMatrix();
// expect
EXPECT_EQ(0, STATE(SMALL_ANGLE));
EXPECT_EQ(true, isUpright());
}
// STUBS