From b3b3b6a7d6b07f7e605a8de5f4cdd4fd2954681e Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Mon, 18 Nov 2019 09:40:31 -0500 Subject: [PATCH] Fix small angle check reversed logic --- src/main/flight/imu.c | 2 +- src/test/unit/flight_imu_unittest.cc | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 5f3c2b3efd..5d4e9e947b 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -724,5 +724,5 @@ void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v) bool isUpright(void) { - return attitudeIsEstablished && getCosTiltAngle() <= smallAngleCosZ; + return attitudeIsEstablished && getCosTiltAngle() > smallAngleCosZ; } diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index c4790e6cc2..a2bcf4e451 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -175,7 +175,7 @@ TEST(FlightImuTest, TestSmallAngle) imuComputeRotationMatrix(); // expect - EXPECT_EQ(true, isUpright()); + EXPECT_EQ(false, isUpright()); // given rMat[0][0] = r1; @@ -187,7 +187,7 @@ TEST(FlightImuTest, TestSmallAngle) imuComputeRotationMatrix(); // expect - EXPECT_EQ(true, isUpright()); + EXPECT_EQ(false, isUpright()); // given memset(rMat, 0.0, sizeof(float) * 9); @@ -196,7 +196,7 @@ TEST(FlightImuTest, TestSmallAngle) imuComputeRotationMatrix(); // expect - EXPECT_EQ(true, isUpright()); + EXPECT_EQ(false, isUpright()); } // STUBS