Adding test for the downwards thrust direction logic added in

495c6b8f73.

src/flight_common.c

@@ -68,7 +68,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     for (axis = 0; axis < 3; axis++) {
         if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
             // 50 degrees max inclination
-            errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle[axis] + angleTrim->raw[axis];
+            errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle.raw[axis] + angleTrim->raw[axis];
             PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768   16 bits is ok for result
             PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
 
@@ -127,7 +127,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         // -----Get the desired angle rate depending on flight mode
         if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { // MODE relying on ACC
             // calculate error and limit the angle to max configured inclination
-            errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle[axis] + angleTrim->raw[axis]; // 16 bits is ok here
+            errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here
         }
         if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
             AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[2]) >> 5);

src/flight_common.h

@@ -28,8 +28,6 @@ enum {
 
 #define ANGLE_INDEX_COUNT 2
 
-extern int16_t angle[ANGLE_INDEX_COUNT]; // see angle_index_t
-
 // See http://en.wikipedia.org/wiki/Flight_dynamics
 enum {
     FD_ROLL = 0,
@@ -82,6 +80,20 @@ typedef union {
     rollAndPitchTrims_t_def trims;
 } rollAndPitchTrims_t;
 
+typedef struct angleInclinations_s {
+    // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
+    int16_t rollDeciDegrees;
+    int16_t pitchDeciDegrees;
+} angleInclinations_t_def;
+
+typedef union {
+    int16_t raw[ANGLE_INDEX_COUNT];
+    angleInclinations_t_def angles;
+} angleInclinations_t;
+
+
+extern angleInclinations_t angle;
+
 extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];
 extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
 

src/flight_imu.c

@@ -4,7 +4,7 @@
 
 #include "common/maths.h"
 
-#include "platform.h"
+#include <platform.h>
 
 #include "common/axis.h"
 #include "flight_common.h"
@@ -68,7 +68,7 @@ float magneticDeclination = 0.0f;       // calculated at startup from config
 int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
-int16_t angle[2] = { 0, 0 };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
+angleInclinations_t angle = { { 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 float anglerad[2] = { 0.0f, 0.0f };    // absolute angle inclination in radians
 
 static void getEstimatedAttitude(void);
@@ -324,8 +324,8 @@ static void getEstimatedAttitude(void)
     // Attitude of the estimated vector
     anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
     anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
-    angle[AI_ROLL] = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
+    angle.angles.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
-    angle[AI_PITCH] = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
+    angle.angles.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
 
     if (sensors(SENSOR_MAG))
         heading = calculateHeading(&EstM);
@@ -353,6 +353,14 @@ static void getEstimatedAttitude(void)
 #ifdef BARO
 #define UPDATE_INTERVAL 25000   // 40hz update rate (20hz LPF on acc)
 
+
+#define DEGREES_80_IN_DECIDEGREES 800
+
+bool isThrustFacingDownwards(angleInclinations_t *angleInclinations)
+{
+    return abs(angleInclinations->angles.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(angleInclinations->angles.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
+}
+
 int getEstimatedAltitude(void)
 {
     static uint32_t previousT;
@@ -415,7 +423,9 @@ int getEstimatedAltitude(void)
 
     // set vario
     vario = applyDeadband(vel_tmp, 5);
-    if (abs(angle[ROLL]) < 800 && abs(angle[PITCH]) < 800) { // only calculate pid if the copters thrust is facing downwards(<80deg)
+
+    // only calculate pid if the copters thrust is facing downwards
+    if (isThrustFacingDownwards(&angle)) {
 		// Altitude P-Controller
 		error = constrain(AltHold - EstAlt, -500, 500);
 		error = applyDeadband(error, 10);       // remove small P parametr to reduce noise near zero position

src/flight_mixer.c

@@ -448,8 +448,8 @@ void mixTable(void)
             break;
 
         case MULTITYPE_GIMBAL:
-            servo[0] = (((int32_t)servoConf[0].rate * angle[PITCH]) / 50) + determineServoMiddleOrForwardFromChannel(0);
+            servo[0] = (((int32_t)servoConf[0].rate * angle.angles.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(0);
-            servo[1] = (((int32_t)servoConf[1].rate * angle[ROLL]) / 50) + determineServoMiddleOrForwardFromChannel(1);
+            servo[1] = (((int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(1);
             break;
 
         case MULTITYPE_AIRPLANE:
@@ -502,11 +502,11 @@ void mixTable(void)
 
         if (rcOptions[BOXCAMSTAB]) {
             if (gimbalConfig->gimbal_flags & GIMBAL_MIXTILT) {
-                servo[0] -= (-(int32_t)servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)servoConf[1].rate * angle[ROLL] / 50;
+                servo[0] -= (-(int32_t)servoConf[0].rate) * angle.angles.pitchDeciDegrees / 50 - (int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees / 50;
-                servo[1] += (-(int32_t)servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)servoConf[1].rate * angle[ROLL] / 50;
+                servo[1] += (-(int32_t)servoConf[0].rate) * angle.angles.pitchDeciDegrees / 50 + (int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees / 50;
             } else {
-                servo[0] += (int32_t)servoConf[0].rate * angle[PITCH] / 50;
+                servo[0] += (int32_t)servoConf[0].rate * angle.angles.pitchDeciDegrees / 50;
-                servo[1] += (int32_t)servoConf[1].rate * angle[ROLL]  / 50;
+                servo[1] += (int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees  / 50;
             }
         }
     }

src/mw.c

@@ -173,7 +173,7 @@ void annexCode(void)
         static uint32_t LEDTime;
         if ((int32_t)(currentTime - LEDTime) >= 0) {
             LEDTime = currentTime + 50000;
-            ledringState(f.ARMED, angle[AI_PITCH], angle[AI_ROLL]);
+            ledringState(f.ARMED, angle.angles.pitchDeciDegrees, angle.angles.rollDeciDegrees);
         }
     }
 #endif

src/platform.h

@@ -33,7 +33,9 @@
 
 #define SENSORS_SET (SENSOR_ACC)
 
-#else
+#endif // STM32F3DISCOVERY
+
+#ifdef STM32F10X_MD
 
 #include "stm32f10x_conf.h"
 #include "core_cm3.h"
@@ -108,4 +110,4 @@
 #endif
 #endif
 
-#endif
+#endif // STM32F10X_MD

src/serial_msp.c

@@ -521,7 +521,7 @@ static void evaluateCommand(void)
     case MSP_ATTITUDE:
         headSerialReply(6);
         for (i = 0; i < 2; i++)
-            serialize16(angle[i]);
+            serialize16(angle.raw[i]);
         serialize16(heading);
         break;
     case MSP_ALTITUDE:

test/Makefile

@@ -30,7 +30,7 @@ CXXFLAGS += -g -Wall -Wextra -pthread
 
 # All tests produced by this Makefile.  Remember to add new tests you
 # created to the list.
-TESTS = battery_unittest
+TESTS = battery_unittest flight_imu_unittest
 
 # All Google Test headers.  Usually you shouldn't change this
 # definition.
@@ -71,6 +71,8 @@ gtest_main.a : gtest-all.o gtest_main.o
 # gtest_main.a, depending on whether it defines its own main()
 # function.
 
+
+
 battery.o : $(USER_DIR)/battery.c $(USER_DIR)/battery.h $(GTEST_HEADERS)
 	$(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $(USER_DIR)/battery.c
 
@@ -81,3 +83,16 @@ battery_unittest.o : $(TEST_DIR)/battery_unittest.cc \
 battery_unittest : battery.o battery_unittest.o gtest_main.a
 	$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $@
 
+
+
+
+flight_imu.o : $(USER_DIR)/flight_imu.c $(USER_DIR)/flight_imu.h $(GTEST_HEADERS)
+	$(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $(USER_DIR)/flight_imu.c -I$(TEST_DIR)
+
+flight_imu_unittest.o : $(TEST_DIR)/flight_imu_unittest.cc \
+                     $(USER_DIR)/flight_imu.h $(GTEST_HEADERS)
+	$(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $(TEST_DIR)/flight_imu_unittest.cc -I$(USER_DIR)
+
+flight_imu_unittest : flight_imu.o flight_imu_unittest.o gtest_main.a
+	$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $@
+

test/battery_unittest.cc

@@ -2,6 +2,8 @@
 
 #include <limits.h>
 #include "battery.h"
+
+#include "unittest_macros.h"
 #include "gtest/gtest.h"
 
 typedef struct batteryAdcToVoltageExpectation_s {
@@ -44,9 +46,6 @@ TEST(BatteryTest, BatteryADCToVoltage)
 
 // STUBS
 
-#define UNUSED(x) (void)(x)
-
-
 uint16_t adcGetChannel(uint8_t channel)
 {
     UNUSED(channel);

test/flight_imu_unittest.cc

@@ -0,0 +1,128 @@
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <limits.h>
+
+#define BARO
+
+
+// FIXME this giant list of includes (below) and stubs (bottom) indicates there is too much going on in flight_imu.c and that it needs decoupling and breaking up.
+
+#include "common/axis.h"
+#include "flight_common.h"
+
+#include "sensors_common.h"
+#include "drivers/accgyro_common.h"
+#include "sensors_gyro.h"
+#include "sensors_compass.h"
+#include "sensors_acceleration.h"
+#include "sensors_barometer.h"
+
+#include "gps_common.h"
+
+#include "gimbal.h"
+#include "flight_mixer.h"
+
+// FIXME remove dependency on config.h
+#include "boardalignment.h"
+#include "battery.h"
+#include "escservo.h"
+#include "rc_controls.h"
+#include "rx_common.h"
+#include "telemetry_common.h"
+#include "drivers/serial_common.h"
+#include "serial_common.h"
+#include "failsafe.h"
+#include "runtime_config.h"
+#include "config.h"
+#include "config_profile.h"
+#include "config_master.h"
+
+
+#include "flight_imu.h"
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+#define DOWNWARDS_THRUST true
+#define UPWARDS_THRUST false
+
+
+bool isThrustFacingDownwards(angleInclinations_t *angleInclinations);
+
+typedef struct angleInclinationExpectation_s {
+    angleInclinations_t inclination;
+    bool expectDownwardsThrust;
+} angleInclinationExpectation_t;
+
+TEST(FlightImuTest, IsThrustFacingDownwards)
+{
+    // given
+
+    angleInclinationExpectation_t angleInclinationExpectations[] = {
+            { { 0, 0 }, DOWNWARDS_THRUST },
+            { { 799, 799 }, DOWNWARDS_THRUST },
+            { { 800, 799 }, UPWARDS_THRUST },
+            { { 799, 800 }, UPWARDS_THRUST },
+            { { 800, 800 }, UPWARDS_THRUST },
+            { { 801, 801 }, UPWARDS_THRUST },
+            { { -799, -799 }, DOWNWARDS_THRUST },
+            { { -800, -799 }, UPWARDS_THRUST },
+            { { -799, -800 }, UPWARDS_THRUST },
+            { { -800, -800 }, UPWARDS_THRUST },
+            { { -801, -801 }, UPWARDS_THRUST }
+    };
+    uint8_t testIterationCount = sizeof(angleInclinationExpectations) / sizeof(angleInclinationExpectation_t);
+
+    // expect
+
+    for (uint8_t index = 0; index < testIterationCount; index ++) {
+        angleInclinationExpectation_t *angleInclinationExpectation = &angleInclinationExpectations[index];
+        printf("iteration: %d\n", index);
+        bool result = isThrustFacingDownwards(&angleInclinationExpectation->inclination);
+        EXPECT_EQ(angleInclinationExpectation->expectDownwardsThrust, result);
+    }
+}
+
+TEST(FlightImuTest, IsThrustFacingUpwards)
+{
+    angleInclinations_t angleInclinations = { { 800, 800 } };
+
+    bool result = isThrustFacingDownwards(&angleInclinations);
+
+    EXPECT_EQ(false, result);
+}
+
+// STUBS
+
+uint16_t acc_1G;
+profile_t currentProfile;
+master_t masterConfig;
+int16_t heading;
+flags_t f;
+gyro_t gyro;
+int16_t magADC[XYZ_AXIS_COUNT];
+int32_t BaroAlt;
+
+void gyroGetADC(void) {};
+bool sensors(uint32_t mask)
+{
+    UNUSED(mask);
+    return false;
+};
+void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
+{
+    UNUSED(rollAndPitchTrims);
+}
+
+uint32_t micros(void) { return 0; }
+bool isBaroCalibrationComplete(void) { return true; }
+void performBaroCalibrationCycle(void) {}
+int32_t baroCalculateAltitude(void) { return 0; }
+int constrain(int amt, int low, int high)
+{
+    UNUSED(amt);
+    UNUSED(low);
+    UNUSED(high);
+    return 0;
+}

test/unittest_macros.h

@@ -0,0 +1,4 @@
+#pragma once
+
+
+#define UNUSED(x) (void)(x)