From 61392c5afca27efba162fc400f400f55f57d618d Mon Sep 17 00:00:00 2001 From: JOhn Aughey Date: Thu, 28 Apr 2016 12:55:23 -0500 Subject: [PATCH 1/6] Removing inline from constrin and constrainf. These functions are used elsewhere and the inline declaration can cause the compiler to make them static and unavailable outside of this file. Unless these functions are defined in a .h file, they cannot be inlined. --- src/main/common/maths.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 3be8eff72..f54aeacdb 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -111,7 +111,7 @@ int32_t applyDeadband(int32_t value, int32_t deadband) return value; } -inline int constrain(int amt, int low, int high) +int constrain(int amt, int low, int high) { if (amt < low) return low; @@ -121,7 +121,7 @@ inline int constrain(int amt, int low, int high) return amt; } -inline float constrainf(float amt, float low, float high) +float constrainf(float amt, float low, float high) { if (amt < low) return low; From b3216439de1a2023efb352eb34506ea4cc540f6e Mon Sep 17 00:00:00 2001 From: JOhn Aughey Date: Thu, 28 Apr 2016 12:57:14 -0500 Subject: [PATCH 2/6] Allow the address of the start of the flash space to be defined externally. For testing, the flash memory can be allocated and defined to a different region than on an embedded processor. --- src/main/config/config.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/config/config.c b/src/main/config/config.c index 116692ee1..07cb5abba 100755 --- a/src/main/config/config.c +++ b/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; From ac11732a869f57be1280938ab44acfaa0294e2b4 Mon Sep 17 00:00:00 2001 From: JOhn Aughey Date: Thu, 28 Apr 2016 12:58:37 -0500 Subject: [PATCH 3/6] Separate the initialization and main step into two different functions. For testing, it is useful to have the loop be separated from the initialization so that an external function can step through the main loop. --- src/main/main.c | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index 49fc7c246..56e9077d4 100644 --- a/src/main/main.c +++ b/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) { - scheduler(); - processLoopback(); +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/ From 091aa24249a81ee70d4848513f1b15ecd9ccb906 Mon Sep 17 00:00:00 2001 From: JOhn Aughey Date: Thu, 28 Apr 2016 13:00:16 -0500 Subject: [PATCH 4/6] For fake gyro/acc, set the output to an fake variable. For testing, the gyros and accelerometer values might needed to be artifically set. This change allows a testing loop to set the gyro and acc to values through a global variable. --- src/main/sensors/initialisation.c | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 9c49090d4..415ebbc4a 100755 --- a/src/main/sensors/initialisation.c +++ b/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 Date: Thu, 28 Apr 2016 13:50:03 -0500 Subject: [PATCH 5/6] Fixing null deref error when VBAT is not set. If VBAT is not configured, batteryConfig is null. This will possibly set the vbatPidCompensation to an unknown value. This change checkes for the validity of batteryConfig before defining the values to be used. --- src/main/flight/mixer.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 802e7e780..813ec50f5 100755 --- a/src/main/flight/mixer.c +++ b/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]; From b6ff69fedafcd9fd78ac9ea67e21736365e0edc5 Mon Sep 17 00:00:00 2001 From: JOhn Aughey Date: Mon, 2 May 2016 10:03:43 -0500 Subject: [PATCH 6/6] Moving constrain and constrainf to the include file to be inlined. --- src/main/common/maths.c | 20 -------------------- src/main/common/maths.h | 22 ++++++++++++++++++++++ 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index f54aeacdb..888847e07 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -111,26 +111,6 @@ int32_t applyDeadband(int32_t value, int32_t deadband) return value; } -int constrain(int amt, int low, int high) -{ - if (amt < low) - return low; - else if (amt > high) - return high; - else - return amt; -} - -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; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 4a32e282c..95f0ee858 100644 --- a/src/main/common/maths.h +++ b/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; +}