diff --git a/Makefile b/Makefile
index d5bfb1d006..4763aa787f 100644
--- a/Makefile
+++ b/Makefile
@@ -255,6 +255,9 @@ endif
ifeq ($(TARGET),$(filter $(TARGET), $(CC3D_TARGETS)))
TARGET_FLAGS := $(TARGET_FLAGS) -DCC3D
+ifeq ($(TARGET),CC3D_OPBL)
+TARGET_FLAGS := $(TARGET_FLAGS) -DCC3D_OPBL
+endif
TARGET_DIR = $(ROOT)/src/main/target/CC3D
endif
diff --git a/src/main/config/config.c b/src/main/config/config.c
index dc7abfa8a7..d3759bdd34 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -140,7 +140,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
-static const uint8_t EEPROM_CONF_VERSION = 134;
+static const uint8_t EEPROM_CONF_VERSION = 135;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@@ -190,8 +190,6 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->dterm_lpf_hz = 80; // filtering ON by default
pidProfile->dynamic_pterm = 1;
- pidProfile->H_sensitivity = 75; // TODO - Cleanup during next EEPROM changes
-
#ifdef GTUNE
pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune.
pidProfile->gtune_lolimP[PITCH] = 10; // [0..200] Lower limit of PITCH P during G tune.
@@ -485,7 +483,8 @@ static void resetConf(void)
masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE;
#endif
masterConfig.servo_pwm_rate = 50;
- masterConfig.use_oneshot42 = 0;
+ masterConfig.fast_pwm_protocol = 0;
+ masterConfig.use_unsyncedPwm = 0;
#ifdef CC3D
masterConfig.use_buzzer_p6 = 0;
#endif
@@ -500,11 +499,7 @@ static void resetConf(void)
resetSerialConfig(&masterConfig.serialConfig);
-#if defined(STM32F10X) && !defined(CC3D)
- masterConfig.emf_avoidance = 1;
-#else
- masterConfig.emf_avoidance = 0;
-#endif
+ masterConfig.emf_avoidance = 0; // TODO - needs removal
resetPidProfile(¤tProfile->pidProfile);
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index 281a7fa3d5..25a21afba7 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -34,8 +34,8 @@ typedef struct master_t {
uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz)
uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz)
- uint8_t use_oneshot42; // Oneshot42
- uint8_t use_multiShot; // multishot
+ uint8_t fast_pwm_protocol; // Fast Pwm Protocol
+ uint8_t use_unsyncedPwm; // unsync fast pwm protocol from PID loop
#ifdef USE_SERVOS
servoMixer_t customServoMixer[MAX_SERVO_RULES];
diff --git a/src/main/debug.h b/src/main/debug.h
index facb9982ec..4f6250b758 100644
--- a/src/main/debug.h
+++ b/src/main/debug.h
@@ -42,10 +42,13 @@ extern uint32_t sectionTimes[2][4];
#endif
typedef enum {
- DEBUG_CYCLETIME = 1,
+ DEBUG_NONE,
+ DEBUG_CYCLETIME,
DEBUG_BATTERY,
DEBUG_GYRO,
DEBUG_ACCELEROMETER,
DEBUG_MIXER,
- DEBUG_AIRMODE
+ DEBUG_AIRMODE,
+ DEBUG_PIDLOOP,
+ DEBUG_COUNT
} debugType_e;
diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index 9c53863270..0ce82b5184 100755
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -31,8 +31,7 @@
void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
-void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t useOneshot42);
-void pwmMultiShotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex);
+void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint8_t useUnsyncedPwm, uint16_t idlePulse);
void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
/*
@@ -912,7 +911,7 @@ if (init->useBuzzerP6) {
if (type == MAP_TO_PPM_INPUT) {
#if defined(SPARKY) || defined(ALIENFLIGHTF3)
- if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) {
+ if (init->useFastPwm || isMotorBrushed(init->motorPwmRate)) {
ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2);
}
#endif
@@ -923,18 +922,14 @@ if (init->useBuzzerP6) {
} else if (type == MAP_TO_MOTOR_OUTPUT) {
#ifdef CC3D
- if (init->useOneshot || isMotorBrushed(init->motorPwmRate)){
+ if (init->useFastPwm || isMotorBrushed(init->motorPwmRate)){
// Skip it if it would cause PPM capture timer to be reconfigured or manually overflowed
if (timerHardwarePtr->tim == TIM2)
continue;
}
#endif
- if (init->useOneshot) {
- if (init->useMultiShot) {
- pwmMultiShotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount);
- } else {
- pwmOneshotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->useOneshot42);
- }
+ if (init->useFastPwm) {
+ pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->fastPwmProtocolType, init->motorPwmRate, init->useUnsyncedPwm, init->idlePulse);
pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_ONESHOT|PWM_PF_OUTPUT_PROTOCOL_PWM ;
} else if (isMotorBrushed(init->motorPwmRate)) {
pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse);
diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
index 9d8377f17c..1edf1458d3 100644
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@@ -36,9 +36,11 @@
#define MAX_INPUTS 8
#define PWM_TIMER_MHZ 1
-#define ONESHOT_TIMER_MHZ 24
+
#define PWM_BRUSHED_TIMER_MHZ 8
#define MULTISHOT_TIMER_MHZ 12
+#define ONESHOT42_TIMER_MHZ 24
+#define ONESHOT125_TIMER_MHZ 8
typedef struct sonarGPIOConfig_s {
GPIO_TypeDef *gpio;
@@ -59,9 +61,8 @@ typedef struct drv_pwm_config_s {
bool useUART3;
#endif
bool useVbat;
- bool useOneshot;
- bool useOneshot42;
- bool useMultiShot;
+ bool useFastPwm;
+ bool useUnsyncedPwm;
bool useSoftSerial;
bool useLEDStrip;
#ifdef SONAR
@@ -70,11 +71,12 @@ typedef struct drv_pwm_config_s {
#ifdef USE_SERVOS
bool useServos;
bool useChannelForwarding; // configure additional channels as servos
-#ifdef CC3D
- bool useBuzzerP6;
-#endif
+ uint8_t fastPwmProtocolType;
uint16_t servoPwmRate;
uint16_t servoCenterPulse;
+#endif
+#ifdef CC3D
+ bool useBuzzerP6;
#endif
bool airplane; // fixed wing hardware config, lots of servos etc
uint16_t motorPwmRate;
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 97a8fef655..56759cbd4b 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -135,27 +135,6 @@ static void pwmWriteStandard(uint8_t index, uint16_t value)
{
*motors[index]->ccr = value;
}
-#if defined(STM32F10X) && !defined(CC3D)
-static void pwmWriteOneshot125(uint8_t index, uint16_t value)
-{
- *motors[index]->ccr = value * 21 / 6; // 24Mhz -> 8Mhz
-}
-
-static void pwmWriteOneshot42(uint8_t index, uint16_t value)
-{
- *motors[index]->ccr = value * 7 / 6;
-}
-#else
-static void pwmWriteOneshot125(uint8_t index, uint16_t value)
-{
- *motors[index]->ccr = value * 3; // 24Mhz -> 8Mhz
-}
-
-static void pwmWriteOneshot42(uint8_t index, uint16_t value)
-{
- *motors[index]->ccr = value;
-}
-#endif
static void pwmWriteMultiShot(uint8_t index, uint16_t value)
{
@@ -227,20 +206,30 @@ void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motor
motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
}
-void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t useOneshot42)
+void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint8_t useUnsyncedPwm, uint16_t idlePulse)
{
- motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT_TIMER_MHZ, 0xFFFF, 0);
- if (useOneshot42) {
- motors[motorIndex]->pwmWritePtr = pwmWriteOneshot42;
- } else {
- motors[motorIndex]->pwmWritePtr = pwmWriteOneshot125;
- }
-}
+ uint32_t timerMhzCounter;
-void pwmMultiShotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex)
-{
- motors[motorIndex] = pwmOutConfig(timerHardware, MULTISHOT_TIMER_MHZ, 0xFFFF, 0);
- motors[motorIndex]->pwmWritePtr = pwmWriteMultiShot;
+ switch (fastPwmProtocolType) {
+ default:
+ case (PWM_TYPE_ONESHOT125):
+ timerMhzCounter = ONESHOT125_TIMER_MHZ;
+ break;
+ case (PWM_TYPE_ONESHOT42):
+ timerMhzCounter = ONESHOT42_TIMER_MHZ;
+ break;
+ case (PWM_TYPE_MULTISHOT):
+ timerMhzCounter = MULTISHOT_TIMER_MHZ;
+ }
+
+ if (useUnsyncedPwm) {
+ uint32_t hz = timerMhzCounter * 1000000;
+ motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse);
+ } else {
+ motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, 0xFFFF, 0);
+ }
+
+ motors[motorIndex]->pwmWritePtr = (fastPwmProtocolType == PWM_TYPE_MULTISHOT) ? pwmWriteMultiShot : pwmWriteStandard;
}
#ifdef USE_SERVOS
diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h
index 11036d5278..615ffa7ac2 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -17,6 +17,12 @@
#pragma once
+typedef enum {
+ PWM_TYPE_ONESHOT125 = 0,
+ PWM_TYPE_ONESHOT42 = 1,
+ PWM_TYPE_MULTISHOT = 2
+} FastPwmProtocolTypes_e;
+
void pwmWriteMotor(uint8_t index, uint16_t value);
void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
void pwmCompleteOneshotMotorUpdate(uint8_t motorCount);
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 6240d85532..17717247c4 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -633,7 +633,7 @@ void writeServos(void)
}
#endif
-void writeMotors(void)
+void writeMotors(uint8_t unsyncedPwm)
{
uint8_t i;
@@ -641,7 +641,7 @@ void writeMotors(void)
pwmWriteMotor(i, motor[i]);
- if (feature(FEATURE_ONESHOT125)) {
+ if (feature(FEATURE_ONESHOT125) && !unsyncedPwm) {
pwmCompleteOneshotMotorUpdate(motorCount);
}
}
@@ -653,7 +653,7 @@ void writeAllMotors(int16_t mc)
// Sends commands to all motors
for (i = 0; i < motorCount; i++)
motor[i] = mc;
- writeMotors();
+ writeMotors(1);
}
void stopMotors(void)
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 36dca92263..50cb3eb1cd 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -218,6 +218,6 @@ int servoDirection(int servoIndex, int fromChannel);
#endif
void mixerResetDisarmedMotors(void);
void mixTable(void);
-void writeMotors(void);
+void writeMotors(uint8_t unsyncedPwm);
void stopMotors(void);
void StopPwmAllMotors(void);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 091eda0876..0a5cf87db5 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -62,9 +62,10 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
uint8_t PIDweight[3];
static int32_t errorGyroI[3], errorGyroILimit[3];
+#ifndef SKIP_PID_LUXFLOAT
static float errorGyroIf[3], errorGyroIfLimit[3];
-static int32_t errorAngleI[2];
-static float errorAngleIf[2];
+#endif
+
static bool lowThrottlePidReduction;
static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
@@ -103,22 +104,15 @@ uint16_t getDynamicKp(int axis, pidProfile_t *pidProfile) {
return dynamicKp;
}
-void pidResetErrorAngle(void)
-{
- errorAngleI[ROLL] = 0;
- errorAngleI[PITCH] = 0;
-
- errorAngleIf[ROLL] = 0.0f;
- errorAngleIf[PITCH] = 0.0f;
-}
-
void pidResetErrorGyroState(uint8_t resetOption)
{
if (resetOption >= RESET_ITERM) {
int axis;
for (axis = 0; axis < 3; axis++) {
errorGyroI[axis] = 0;
+#ifndef SKIP_PID_LUXFLOAT
errorGyroIf[axis] = 0.0f;
+#endif
}
}
@@ -141,6 +135,7 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static filterStatePt1_t deltaFilterState[3];
static filterStatePt1_t yawFilterState;
+#ifndef SKIP_PID_LUXFLOAT
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
@@ -280,6 +275,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
#endif
}
}
+#endif
static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
@@ -426,8 +422,10 @@ void pidSetController(pidControllerType_e type)
case PID_CONTROLLER_MWREWRITE:
pid_controller = pidMultiWiiRewrite;
break;
+#ifndef SKIP_PID_LUXFLOAT
case PID_CONTROLLER_LUX_FLOAT:
pid_controller = pidLuxFloat;
+#endif
}
}
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 9043e1e7bb..361ff9508d 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -97,7 +97,6 @@ bool antiWindupProtection;
extern uint32_t targetPidLooptime;
void pidSetController(pidControllerType_e type);
-void pidResetErrorAngle(void);
void pidResetErrorGyroState(uint8_t resetOption);
void setTargetPidLooptime(uint8_t pidProcessDenom);
diff --git a/src/main/io/display.c b/src/main/io/display.c
index 33fc5e5fcf..ed394d9c95 100644
--- a/src/main/io/display.c
+++ b/src/main/io/display.c
@@ -22,6 +22,7 @@
#include "platform.h"
#include "version.h"
+#include "debug.h"
#include "build_config.h"
@@ -58,14 +59,16 @@
#include "flight/navigation.h"
#endif
-#include "config/runtime_config.h"
-
#include "config/config.h"
+#include "config/runtime_config.h"
+#include "config/config_profile.h"
#include "display.h"
#include "scheduler.h"
+extern profile_t *currentProfile;
+
controlRateConfig_t *getControlRateConfig(uint8_t profileIndex);
#define MICROSECONDS_IN_A_SECOND (1000 * 1000)
@@ -317,13 +320,26 @@ void showProfilePage(void)
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
- uint8_t currentRateProfileIndex = getCurrentControlRateProfile();
+ static const char* const axisTitles[3] = {"ROL", "PIT", "YAW"};
+ const pidProfile_t *pidProfile = ¤tProfile->pidProfile;
+ for (int axis = 0; axis < 3; ++axis) {
+ tfp_sprintf(lineBuffer, "%s P:%3d I:%3d D:%3d",
+ axisTitles[axis],
+ pidProfile->P8[axis],
+ pidProfile->I8[axis],
+ pidProfile->D8[axis]
+ );
+ padLineBuffer();
+ i2c_OLED_set_line(rowIndex++);
+ i2c_OLED_send_string(lineBuffer);
+ }
+
+ const uint8_t currentRateProfileIndex = getCurrentControlRateProfile();
tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex);
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
- controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex);
-
+ const controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex);
tfp_sprintf(lineBuffer, "RCE: %d, RCR: %d",
controlRateConfig->rcExpo8,
controlRateConfig->rcRate8
diff --git a/src/main/io/display.h b/src/main/io/display.h
index 7d420b6563..8149a35ade 100644
--- a/src/main/io/display.h
+++ b/src/main/io/display.h
@@ -15,7 +15,7 @@
* along with Cleanflight. If not, see .
*/
-//#define ENABLE_DEBUG_OLED_PAGE
+#define ENABLE_DEBUG_OLED_PAGE
typedef enum {
PAGE_WELCOME,
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 3f0bb134dc..423c59bc4e 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -26,6 +26,7 @@
#include "platform.h"
#include "scheduler.h"
#include "version.h"
+#include "debug.h"
#include "build_config.h"
@@ -426,20 +427,25 @@ static const char * const lookupTableMagHardware[] = {
"AK8963"
};
-static const char * const lookupTableDebug[] = {
+static const char * const lookupTableDebug[DEBUG_COUNT] = {
"NONE",
"CYCLETIME",
"BATTERY",
"GYRO",
"ACCELEROMETER",
"MIXER",
- "AIRMODE"
+ "AIRMODE",
+ "PIDLOOP",
};
static const char * const lookupTableSuperExpoYaw[] = {
"OFF", "ON", "ALWAYS"
};
+static const char * const lookupTableFastPwm[] = {
+ "ONESHOT125", "ONESHOT42", "MULTISHOT"
+};
+
typedef struct lookupTableEntry_s {
const char * const *values;
const uint8_t valueCount;
@@ -466,6 +472,7 @@ typedef enum {
TABLE_MAG_HARDWARE,
TABLE_DEBUG,
TABLE_SUPEREXPO_YAW,
+ TABLE_FAST_PWM,
} lookupTableIndex_e;
static const lookupTableEntry_t lookupTables[] = {
@@ -488,7 +495,8 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) },
{ lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) },
{ lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) },
- { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) }
+ { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) },
+ { lookupTableFastPwm, sizeof(lookupTableFastPwm) / sizeof(char *) },
};
#define VALUE_TYPE_OFFSET 0
@@ -564,12 +572,12 @@ const clivalue_t valueTable[] = {
{ "3d_neutral", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
- { "use_oneshot42", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_oneshot42, .config.lookup = { TABLE_OFF_ON } },
- { "use_multishot", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_multiShot, .config.lookup = { TABLE_OFF_ON } },
+ { "unsynced_fast_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_unsyncedPwm, .config.lookup = { TABLE_OFF_ON } },
+ { "fast_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.fast_pwm_protocol, .config.lookup = { TABLE_FAST_PWM } },
#ifdef CC3D
{ "enable_buzzer_p6", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_buzzer_p6, .config.lookup = { TABLE_OFF_ON } },
#endif
- { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motor_pwm_rate, .config.minmax = { 300, 32000 } },
+ { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motor_pwm_rate, .config.minmax = { 200, 32000 } },
{ "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servo_pwm_rate, .config.minmax = { 50, 498 } },
{ "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index d44dc8e9aa..aa0d23b8c5 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -172,7 +172,8 @@ void setGyroSamplingSpeed(uint16_t looptime) {
}
#endif
- if (!(masterConfig.use_multiShot || masterConfig.use_oneshot42) && ((masterConfig.gyro_sync_denom * gyroSampleRate) == 125)) masterConfig.pid_process_denom = 3;
+ // Oneshot125 protection
+ if ((masterConfig.fast_pwm_protocol == 0) && ((masterConfig.gyro_sync_denom * gyroSampleRate) == 125) && masterConfig.pid_process_denom < 3) masterConfig.pid_process_denom = 3;
}
}
@@ -1861,6 +1862,10 @@ void mspProcess(void)
waitForSerialPortToFinishTransmitting(candidatePort->port);
stopMotors();
handleOneshotFeatureChangeOnRestart();
+ // On real flight controllers, systemReset() will do a soft reset of the device,
+ // reloading the program. But to support offline testing this flag needs to be
+ // cleared so that the software doesn't continuously attempt to reboot itself.
+ isRebootScheduled = false;
systemReset();
}
}
diff --git a/src/main/main.c b/src/main/main.c
index ab02994433..06a7e99c3f 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -176,7 +176,7 @@ void init(void)
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
- SetSysClock(masterConfig.emf_avoidance);
+ SetSysClock(0); // TODO - Remove from config in the future
#endif
//i2cSetOverclock(masterConfig.i2c_overclock);
@@ -311,18 +311,14 @@ void init(void)
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
- pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
- if (masterConfig.use_oneshot42) {
- pwm_params.useOneshot42 = masterConfig.use_oneshot42 ? true : false;
- masterConfig.use_multiShot = false;
- } else {
- pwm_params.useMultiShot = masterConfig.use_multiShot ? true : false;
- }
+ pwm_params.useFastPwm = feature(FEATURE_ONESHOT125); // Configurator feature abused for enabling Fast PWM
+ pwm_params.fastPwmProtocolType = masterConfig.fast_pwm_protocol;
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
+ pwm_params.useUnsyncedPwm = masterConfig.use_unsyncedPwm;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
- if (pwm_params.motorPwmRate > 500)
+ if (pwm_params.motorPwmRate > 500 && !pwm_params.useFastPwm)
pwm_params.idlePulse = 0; // brushed motors
#ifdef CC3D
pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false;
diff --git a/src/main/mw.c b/src/main/mw.c
index 204ad70ebc..59ba692fc6 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -504,7 +504,6 @@ void processRx(void)
} else {
pidResetErrorGyroState(RESET_ITERM_AND_REDUCE_PID);
}
- pidResetErrorAngle();
}
} else {
pidResetErrorGyroState(RESET_DISABLE);
@@ -655,6 +654,7 @@ void processRx(void)
void subTaskPidController(void)
{
+ const uint32_t startTime = micros();
// PID - note this is function pointer set by setPIDController()
pid_controller(
¤tProfile->pidProfile,
@@ -663,10 +663,13 @@ void subTaskPidController(void)
&masterConfig.accelerometerTrims,
&masterConfig.rxConfig
);
+ if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;}
}
void subTaskMainSubprocesses(void) {
+ const uint32_t startTime = micros();
+
if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
filterRc();
}
@@ -730,16 +733,18 @@ void subTaskMainSubprocesses(void) {
#ifdef TRANSPONDER
updateTransponder();
#endif
+ if (debugMode == DEBUG_PIDLOOP) {debug[1] = micros() - startTime;}
}
void subTaskMotorUpdate(void)
{
+ const uint32_t startTime = micros();
if (debugMode == DEBUG_CYCLETIME) {
static uint32_t previousMotorUpdateTime;
- uint32_t currentDeltaTime = micros() - previousMotorUpdateTime;
+ const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime;
debug[2] = currentDeltaTime;
debug[3] = currentDeltaTime - targetPidLooptime;
- previousMotorUpdateTime = micros();
+ previousMotorUpdateTime = startTime;
}
mixTable();
@@ -750,8 +755,9 @@ void subTaskMotorUpdate(void)
#endif
if (motorControlEnable) {
- writeMotors();
+ writeMotors(masterConfig.use_unsyncedPwm);
}
+ if (debugMode == DEBUG_PIDLOOP) {debug[3] = micros() - startTime;}
}
uint8_t setPidUpdateCountDown(void) {
@@ -778,10 +784,12 @@ void taskMainPidLoopCheck(void)
debug[1] = averageSystemLoadPercent;
}
+ const uint32_t startTime = micros();
while (true) {
if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
static uint8_t pidUpdateCountdown;
+ if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting
if (runTaskMainSubprocesses) {
subTaskMainSubprocesses();
runTaskMainSubprocesses = false;
diff --git a/src/main/scheduler.h b/src/main/scheduler.h
index 34141fd820..237776c259 100755
--- a/src/main/scheduler.h
+++ b/src/main/scheduler.h
@@ -44,14 +44,14 @@ typedef enum {
/* Actual tasks */
TASK_SYSTEM = 0,
TASK_GYROPID,
- TASK_ATTITUDE,
TASK_ACCEL,
+ TASK_ATTITUDE,
+ TASK_RX,
TASK_SERIAL,
+ TASK_BATTERY,
#ifdef BEEPER
TASK_BEEPER,
#endif
- TASK_BATTERY,
- TASK_RX,
#ifdef GPS
TASK_GPS,
#endif
diff --git a/src/main/scheduler_tasks.c b/src/main/scheduler_tasks.c
index 223eadc03d..b9babb4b33 100644
--- a/src/main/scheduler_tasks.c
+++ b/src/main/scheduler_tasks.c
@@ -22,14 +22,15 @@
#include
#include "scheduler.h"
+void taskSystem(void);
void taskMainPidLoopCheck(void);
void taskUpdateAccelerometer(void);
-void taskHandleSerial(void);
void taskUpdateAttitude(void);
-void taskUpdateBeeper(void);
-void taskUpdateBattery(void);
bool taskUpdateRxCheck(uint32_t currentDeltaTime);
void taskUpdateRxMain(void);
+void taskHandleSerial(void);
+void taskUpdateBattery(void);
+void taskUpdateBeeper(void);
void taskProcessGPS(void);
void taskUpdateCompass(void);
void taskUpdateBaro(void);
@@ -39,7 +40,6 @@ void taskUpdateDisplay(void);
void taskTelemetry(void);
void taskLedStrip(void);
void taskTransponder(void);
-void taskSystem(void);
#ifdef USE_BST
void taskBstReadWrite(void);
void taskBstMasterProcess(void);
@@ -61,13 +61,6 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_REALTIME,
},
- [TASK_ATTITUDE] = {
- .taskName = "ATTITUDE",
- .taskFunc = taskUpdateAttitude,
- .desiredPeriod = 1000000 / 100,
- .staticPriority = TASK_PRIORITY_MEDIUM,
- },
-
[TASK_ACCEL] = {
.taskName = "ACCEL",
.taskFunc = taskUpdateAccelerometer,
@@ -75,6 +68,21 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_MEDIUM,
},
+ [TASK_ATTITUDE] = {
+ .taskName = "ATTITUDE",
+ .taskFunc = taskUpdateAttitude,
+ .desiredPeriod = 1000000 / 100,
+ .staticPriority = TASK_PRIORITY_MEDIUM,
+ },
+
+ [TASK_RX] = {
+ .taskName = "RX",
+ .checkFunc = taskUpdateRxCheck,
+ .taskFunc = taskUpdateRxMain,
+ .desiredPeriod = 1000000 / 50, // If event-based scheduling doesn't work, fallback to periodic scheduling
+ .staticPriority = TASK_PRIORITY_HIGH,
+ },
+
[TASK_SERIAL] = {
.taskName = "SERIAL",
.taskFunc = taskHandleSerial,
@@ -82,6 +90,13 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_LOW,
},
+ [TASK_BATTERY] = {
+ .taskName = "BATTERY",
+ .taskFunc = taskUpdateBattery,
+ .desiredPeriod = 1000000 / 50, // 50 Hz
+ .staticPriority = TASK_PRIORITY_LOW,
+ },
+
#ifdef BEEPER
[TASK_BEEPER] = {
.taskName = "BEEPER",
@@ -91,21 +106,6 @@ cfTask_t cfTasks[TASK_COUNT] = {
},
#endif
- [TASK_BATTERY] = {
- .taskName = "BATTERY",
- .taskFunc = taskUpdateBattery,
- .desiredPeriod = 1000000 / 50, // 50 Hz
- .staticPriority = TASK_PRIORITY_LOW,
- },
-
- [TASK_RX] = {
- .taskName = "RX",
- .checkFunc = taskUpdateRxCheck,
- .taskFunc = taskUpdateRxMain,
- .desiredPeriod = 1000000 / 50, // If event-based scheduling doesn't work, fallback to periodic scheduling
- .staticPriority = TASK_PRIORITY_HIGH,
- },
-
#ifdef GPS
[TASK_GPS] = {
.taskName = "GPS",
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 85b5ebb307..7e05f9688a 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -240,6 +240,7 @@ bool fakeGyroDetect(gyro_t *gyro)
gyro->init = fakeGyroInit;
gyro->read = fakeGyroRead;
gyro->temperature = fakeGyroReadTemp;
+ gyro->scale = 1.0f / 16.4f;
return true;
}
#endif
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index caf6e7b880..ca94199b8f 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -71,10 +71,6 @@
#define MAG
#define USE_MAG_HMC5883
-#define INVERTER
-#define BEEPER
-#define DISPLAY
-
#define USE_VCP
#define USE_USART1
#define USE_USART3
@@ -117,28 +113,31 @@
#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER
-#define BLACKBOX
-#define TELEMETRY
-#define SERIAL_RX
-#define SONAR
-#define USE_SERVOS
-#define USE_CLI
-
-#define USE_SERIAL_4WAY_BLHELI_INTERFACE
-
-
-#undef DISPLAY
-#undef SONAR
-//#if defined(OPBL) && defined(USE_SERIAL_1WIRE)
-#undef BARO
-//#undef BLACKBOX
-#undef GPS
-//#endif
-#define SKIP_CLI_COMMAND_HELP
-
#define SPEKTRUM_BIND
// USART3, PB11 (Flexport)
#define BIND_PORT GPIOB
#define BIND_PIN Pin_11
-#define USE_QUATERNION
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define INVERTER
+#define BEEPER
+#define DISPLAY
+#define BLACKBOX
+#define TELEMETRY
+#define SERIAL_RX
+#define USE_SERVOS
+#define USE_CLI
+#define SONAR
+//#define GPS
+
+#undef BARO
+
+#ifdef CC3D_OPBL
+#define SKIP_CLI_COMMAND_HELP
+//#define SKIP_PID_LUXFLOAT
+#undef DISPLAY
+#undef SONAR
+#undef GPS
+#endif
+