/* * This file is part of Cleanflight. * * Cleanflight is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * Cleanflight is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ #include #include #include #include extern "C" { #include "platform.h" #include "build/debug.h" #include "common/axis.h" #include "common/maths.h" #include "config/feature.h" #include "pg/pg.h" #include "pg/pg_ids.h" #include "pg/rx.h" #include "drivers/accgyro/accgyro.h" #include "drivers/compass/compass.h" #include "drivers/sensor.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/mixer.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/position.h" #include "io/gps.h" #include "rx/rx.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/compass.h" #include "sensors/gyro.h" #include "sensors/sensors.h" void imuComputeRotationMatrix(void); void imuUpdateEulerAngles(void); 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); PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = 0 ); } #include "unittest_macros.h" #include "gtest/gtest.h" const float sqrt2over2 = sqrtf(2) / 2.0f; TEST(FlightImuTest, TestCalculateRotationMatrix) { #define TOL 1e-6 // No rotation q.w = 1.0f; q.x = 0.0f; q.y = 0.0f; q.z = 0.0f; imuComputeRotationMatrix(); EXPECT_FLOAT_EQ(1.0f, rMat[0][0]); EXPECT_FLOAT_EQ(0.0f, rMat[0][1]); EXPECT_FLOAT_EQ(0.0f, rMat[0][2]); EXPECT_FLOAT_EQ(0.0f, rMat[1][0]); EXPECT_FLOAT_EQ(1.0f, rMat[1][1]); EXPECT_FLOAT_EQ(0.0f, rMat[1][2]); EXPECT_FLOAT_EQ(0.0f, rMat[2][0]); EXPECT_FLOAT_EQ(0.0f, rMat[2][1]); EXPECT_FLOAT_EQ(1.0f, rMat[2][2]); // 90 degrees around Z axis q.w = sqrt2over2; q.x = 0.0f; q.y = 0.0f; q.z = sqrt2over2; imuComputeRotationMatrix(); EXPECT_NEAR(0.0f, rMat[0][0], TOL); EXPECT_NEAR(-1.0f, rMat[0][1], TOL); EXPECT_NEAR(0.0f, rMat[0][2], TOL); EXPECT_NEAR(1.0f, rMat[1][0], TOL); EXPECT_NEAR(0.0f, rMat[1][1], TOL); EXPECT_NEAR(0.0f, rMat[1][2], TOL); EXPECT_NEAR(0.0f, rMat[2][0], TOL); EXPECT_NEAR(0.0f, rMat[2][1], TOL); EXPECT_NEAR(1.0f, rMat[2][2], TOL); // 60 degrees around X axis q.w = 0.866f; q.x = 0.5f; q.y = 0.0f; q.z = 0.0f; imuComputeRotationMatrix(); EXPECT_NEAR(1.0f, rMat[0][0], TOL); EXPECT_NEAR(0.0f, rMat[0][1], TOL); EXPECT_NEAR(0.0f, rMat[0][2], TOL); EXPECT_NEAR(0.0f, rMat[1][0], TOL); EXPECT_NEAR(0.5f, rMat[1][1], TOL); EXPECT_NEAR(-0.866f, rMat[1][2], TOL); EXPECT_NEAR(0.0f, rMat[2][0], TOL); EXPECT_NEAR(0.866f, rMat[2][1], TOL); EXPECT_NEAR(0.5f, rMat[2][2], TOL); } TEST(FlightImuTest, TestUpdateEulerAngles) { // No rotation memset(rMat, 0.0, sizeof(float) * 9); imuUpdateEulerAngles(); EXPECT_EQ(0, attitude.values.roll); EXPECT_EQ(0, attitude.values.pitch); EXPECT_EQ(0, attitude.values.yaw); // 45 degree yaw memset(rMat, 0.0, sizeof(float) * 9); rMat[0][0] = sqrt2over2; rMat[0][1] = sqrt2over2; rMat[1][0] = -sqrt2over2; rMat[1][1] = sqrt2over2; imuUpdateEulerAngles(); EXPECT_EQ(0, attitude.values.roll); EXPECT_EQ(0, attitude.values.pitch); EXPECT_EQ(450, attitude.values.yaw); } TEST(FlightImuTest, TestSmallAngle) { const float r1 = 0.898; const float r2 = 0.438; // given imuConfigMutable()->small_angle = 25; imuConfigure(0, 0); attitudeIsEstablished = true; // and memset(rMat, 0.0, sizeof(float) * 9); // when imuComputeRotationMatrix(); // expect EXPECT_FALSE(isUpright()); // given rMat[0][0] = r1; rMat[0][2] = r2; rMat[2][0] = -r2; rMat[2][2] = r1; // when imuComputeRotationMatrix(); // expect EXPECT_FALSE(isUpright()); // given memset(rMat, 0.0, sizeof(float) * 9); // when imuComputeRotationMatrix(); // expect EXPECT_FALSE(isUpright()); } // STUBS extern "C" { boxBitmask_t rcModeActivationMask; float rcCommand[4]; float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; gyro_t gyro; acc_t acc; mag_t mag; gpsSolutionData_t gpsSol; int16_t GPS_verticalSpeedInCmS; uint8_t debugMode; int16_t debug[DEBUG16_VALUE_COUNT]; uint8_t stateFlags; uint16_t flightModeFlags; uint8_t armingFlags; pidProfile_t *currentPidProfile; uint16_t enableFlightMode(flightModeFlags_e mask) { return flightModeFlags |= (mask); } uint16_t disableFlightMode(flightModeFlags_e mask) { return flightModeFlags &= ~(mask); } bool sensors(uint32_t mask) { return mask & SENSOR_ACC; }; uint32_t millis(void) { return 0; } uint32_t micros(void) { return 0; } bool compassIsHealthy(void) { return true; } bool baroIsCalibrated(void) { return true; } void performBaroCalibrationCycle(void) {} float baroCalculateAltitude(void) { return 0; } bool gyroGetAccumulationAverage(float *) { return false; } bool accGetAccumulationAverage(float *) { return false; } void mixerSetThrottleAngleCorrection(int) {}; bool gpsRescueIsRunning(void) { return false; } bool isFixedWing(void) { return false; } void pinioBoxTaskControl(void) {} void schedulerIgnoreTaskExecTime(void) {} void schedulerIgnoreTaskStateTime(void) {} void schedulerSetNextStateTime(timeDelta_t) {} bool schedulerGetIgnoreTaskExecTime() { return false; } float gyroGetFilteredDownsampled(int) { return 0.0f; } float baroUpsampleAltitude() { return 0.0f; } float pt2FilterGain(float, float) { return 0.0f; } void pt2FilterInit(pt2Filter_t *baroDerivativeLpf, float) { UNUSED(baroDerivativeLpf); } float pt2FilterApply(pt2Filter_t *baroDerivativeLpf, float) { UNUSED(baroDerivativeLpf); return 0.0f; } }