diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 3be8eff72..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; } -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; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 4a32e282c..3251c237a 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -71,9 +71,6 @@ typedef union { int32_t applyDeadband(int32_t value, int32_t deadband); -int constrain(int amt, int low, int high); -float constrainf(float amt, float low, float high); - void devClear(stdev_t *dev); void devPush(stdev_t *dev, float x); float devVariance(stdev_t *dev); @@ -116,3 +113,23 @@ 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); + +static inline int constrain(int amt, int low, int high) +{ + if (amt < low) + return low; + else if (amt > high) + return high; + else + return amt; +} + +static inline float constrainf(float amt, float low, float high) +{ + if (amt < low) + return low; + else if (amt > high) + return high; + else + return amt; +} diff --git a/src/main/config/config.c b/src/main/config/config.c index 8b389fb2a..dc7abfa8a 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -124,8 +124,14 @@ 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; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 802e7e780..6240d8553 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -751,8 +751,13 @@ STATIC_UNIT_TESTED void servoMixer(void) void mixTable(void) { uint32_t i; - fix12_t vbatCompensationFactor; + fix12_t vbatCompensationFactor = 0; 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,7 +771,7 @@ void mixTable(void) int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero. int16_t rollPitchYawMixMin = 0; - if (batteryConfig->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation(); // Calculate voltage compensation + if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation // Find roll/pitch/yaw desired output for (i = 0; i < motorCount; i++) { @@ -775,7 +780,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]; diff --git a/src/main/main.c b/src/main/main.c index 49fc7c246..ab0299443 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,24 @@ 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/ diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 9c49090d4..85b5ebb30 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,10 @@ 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 +245,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