Merge pull request #372 from aughey/betaflight

Changes to support offline testing

src/main/common/maths.c

@@ -111,26 +111,6 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
     return value;
 }
 
-inline int constrain(int amt, int low, int high)
-{
-    if (amt < low)
-        return low;
-    else if (amt > high)
-        return high;
-    else
-        return amt;
-}
-
-inline float constrainf(float amt, float low, float high)
-{
-    if (amt < low)
-        return low;
-    else if (amt > high)
-        return high;
-    else
-        return amt;
-}
-
 void devClear(stdev_t *dev)
 {
     dev->m_n = 0;

src/main/common/maths.h

@@ -116,3 +116,25 @@ void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
 int16_t qPercent(fix12_t q);
 int16_t qMultiply(fix12_t q, int16_t input);
 fix12_t qConstruct(int16_t num, int16_t den);
+
+// Defining constrain and constrainf as inline in the include file
+// because these functions are used universally and should be fast.
+inline int constrain(int amt, int low, int high)
+{
+    if (amt < low)
+        return low;
+    else if (amt > high)
+        return high;
+    else
+        return amt;
+}
+
+inline float constrainf(float amt, float low, float high)
+{
+    if (amt < low)
+        return low;
+    else if (amt > high)
+        return high;
+    else
+        return amt;
+}

src/main/config/config.c

@@ -124,8 +124,15 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
 #define FLASH_TO_RESERVE_FOR_CONFIG 0x1000
 #endif
 
+// use the last flash pages for storage
+#ifdef CUSTOM_FLASH_MEMORY_ADDRESS
+size_t custom_flash_memory_address = 0;
+#define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address)
+#else
 // use the last flash pages for storage
 #define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG))
+#endif
+
 
 master_t masterConfig;                 // master config struct with data independent from profiles
 profile_t *currentProfile;

src/main/flight/mixer.c

@@ -753,6 +753,11 @@ void mixTable(void)
     uint32_t i;
     fix12_t vbatCompensationFactor;
     static fix12_t mixReduction;
+	bool use_vbat_compensation = false;
+	if (batteryConfig && batteryConfig->vbatPidCompensation) {
+		use_vbat_compensation = true;
+		vbatCompensationFactor = calculateVbatPidCompensation();
+	}
 
     bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
 
@@ -766,8 +771,6 @@ void mixTable(void)
     int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero.
     int16_t rollPitchYawMixMin = 0;
 
-    if (batteryConfig->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation(); // Calculate voltage compensation
-
     // Find roll/pitch/yaw desired output
     for (i = 0; i < motorCount; i++) {
         rollPitchYawMix[i] =
@@ -775,7 +778,7 @@ void mixTable(void)
             axisPID[ROLL] * currentMixer[i].roll +
             -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
 
-        if (batteryConfig->vbatPidCompensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]);  // Add voltage compensation
+        if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]);  // Add voltage compensation
 
         if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
         if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];

src/main/main.c

@@ -26,7 +26,6 @@
 
 #include "common/axis.h"
 #include "common/color.h"
-#include "common/atomic.h"
 #include "common/maths.h"
 
 #include "drivers/nvic.h"
@@ -658,7 +657,7 @@ void processLoopback(void) {
 #define processLoopback()
 #endif
 
-int main(void) {
+void main_init(void) {
     init();
 
     /* Setup scheduler */
@@ -729,12 +728,22 @@ int main(void) {
 #ifdef USE_BST
     setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
 #endif
+}
 
-    while (1) {
+void main_step(void) {
     scheduler();
     processLoopback();
 }
+
+#ifndef NOMAIN
+int main(void)
+{
+    main_init();
+    while(1) {
+        main_step();
     }
+}
+#endif
 
 #ifdef DEBUG_HARDFAULTS
 //from: https://mcuoneclipse.com/2012/11/24/debugging-hard-faults-on-arm-cortex-m/

src/main/sensors/initialisation.c

@@ -214,6 +214,7 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
 }
 
 #ifdef USE_FAKE_GYRO
+int16_t fake_gyro_values[XYZ_AXIS_COUNT] = { 0,0,0 };
 static void fakeGyroInit(uint16_t lpf)
 {
     UNUSED(lpf);
@@ -221,7 +222,9 @@ static void fakeGyroInit(uint16_t lpf)
 
 static bool fakeGyroRead(int16_t *gyroADC)
 {
-    memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
+	for (int i = 0; i < XYZ_AXIS_COUNT; ++i) {
+		gyroADC[i] = fake_gyro_values[i];
+	}
     return true;
 }
 
@@ -241,9 +244,13 @@ bool fakeGyroDetect(gyro_t *gyro)
 #endif
 
 #ifdef USE_FAKE_ACC
+int16_t fake_acc_values[XYZ_AXIS_COUNT] = {0,0,0};
+
 static void fakeAccInit(void) {}
 static bool fakeAccRead(int16_t *accData) {
-    memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
+    for(int i=0;i<XYZ_AXIS_COUNT;++i) {
+		accData[i] = fake_acc_values[i];
+    }
     return true;
 }